精华内容
下载资源
问答
  • 一、传感器时间同步 多传感器融合过程中由于传感器之间的采集频率不同,导致无法保证传感器数据同步。这里以激光雷达为核心传感器,每次收到一次雷达数据,便以当前雷达数据采集时刻作为要插入的时间点,该时刻另一...

    一、传感器时间同步

    多传感器融合过程中由于传感器之间的采集频率不同,导致无法保证传感器数据同步。这里以激光雷达为核心传感器,每次收到一次雷达数据,便以当前雷达数据采集时刻作为要插入的时间点,该时刻另一传感器IMU的数据通过插值获得。这里同样可以参考VINS里相机和IMU时间同步的函数代码getMeasurements()

    主程序在front_end_flow.cpp文件中的ReadData()函数添加。

    ReadData()

    1. 从ros缓冲区中传感器数据取出来,并放进XXX_data_buff_容器中
    2. 传感器同步,另外三种传感器做插值
    3. 如果任意传感器没有插值,剔除该雷达点云帧,找下一个点云帧做融合
      1、读取传感器数据放进XXX_data_buff_容器中
      雷达点云作为主传感器,其他传感器做插值。包含IMU信息(unsynced_imu_)、速度信息(unsynced_velocity_)、GNSS信息(unsynced_gnss_)
    // 雷达点云不需要插值,直接放进cloud_data_buff_容器中
        cloud_sub_ptr_->ParseData(cloud_data_buff_);
        // 其他传感器原始数据放进未做同步的临时容器中unsynced_XXX_
        static std::deque<IMUData> unsynced_imu_;
        static std::deque<VelocityData> unsynced_velocity_;
        static std::deque<GNSSData> unsynced_gnss_;
        // 放进XXX_data_buff_容器中
        imu_sub_ptr_->ParseData(unsynced_imu_);
        velocity_sub_ptr_->ParseData(unsynced_velocity_);
        gnss_sub_ptr_->ParseData(unsynced_gnss_);
        // 点云容器不能为空
        if (cloud_data_buff_.size() == 0)
            return false;
    

    以imu的ParseData()函数为例:

    ParseData()

    读取数据,将新的传感器数据new_imu_data_填充进imu容器imu_data_buff中。

    void IMUSubscriber::ParseData(std::deque<IMUData>& imu_data_buff) {
        if (new_imu_data_.size() > 0) {
            imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end());
            new_imu_data_.clear();
        }
    }
    

    2、传感器同步,另外三种传感器做插值
    cloud_time为主传感器激光雷达的参考时间,然后后面通过SyncData()函数对三个传感器进行插值实现同步操作。具体就是索引和插值。

     double cloud_time = cloud_data_buff_.front().time;
    
        bool valid_imu = IMUData::SyncData(unsynced_imu_, imu_data_buff_, cloud_time);
        bool valid_velocity = VelocityData::SyncData(unsynced_velocity_, velocity_data_buff_, cloud_time);
        bool valid_gnss = GNSSData::SyncData(unsynced_gnss_, gnss_data_buff_, cloud_time);
    
    

    3、如果任意传感器没有插值,剔除该雷达点云帧,找下一个点云帧做融合

     static bool sensor_inited = false;
        if (!sensor_inited) {
            if (!valid_imu || !valid_velocity || !valid_gnss) {
                cloud_data_buff_.pop_front();
                return false;
            }
            sensor_inited = true;
        }
    

    插值SyncData()

    插值的流程是先获得主传感器时间,然后根据索引结果获得这一同步时间前后的两帧数据,根据前后两帧数据的采集时刻,以及要插入的时刻,根据比例获得权重得到另一传感器在同步时间的结果。这里以IMU数据为例,三种传感器流程类似。

    1. 找到与同步时间相邻的左右两个数据,在UnsyncedData.at(0)和.at(1)之间
    2. 根据时间差计算权重线性插值

    输入原始数据,输出线性插值后的数据。

    bool IMUData::SyncData(std::deque<IMUData>& UnsyncedData, std::deque<IMUData>& SyncedData, double sync_time) {
        // 1.保证数据大于2个,同步时间在两个传感器数据UnsyncedData.at(0)和.at(1)中间
        while (UnsyncedData.size() >= 2) {
            // a. 同步时间sync小于传感器第一个数据,失败
            if (UnsyncedData.front().time > sync_time) 
                return false;
            // b. 同步时间sync大于第二个数据,将第二个设置为.at(0)
            if (UnsyncedData.at(1).time < sync_time) {
                UnsyncedData.pop_front();
                continue;
            }
            // c. 距离前一个时间差值较大,说明数据有丢失,前一个不能用
            if (sync_time - UnsyncedData.front().time > 0.2) {
                UnsyncedData.pop_front();
                break;
            }
            // d. 距离后一个时间差值较大,说明数据有丢失,后一个不能用
            if (UnsyncedData.at(1).time - sync_time > 0.2) {
                UnsyncedData.pop_front();
                break;
            }
            break;
        }
        if (UnsyncedData.size() < 2)
            return false;
        // 同步数据synced,前后两帧数据front、back
        IMUData front_data = UnsyncedData.at(0);
        IMUData back_data = UnsyncedData.at(1);
        IMUData synced_data;
        // 2、根据时间差计算权重线性插值
        double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
        double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
        synced_data.time = sync_time;// 同步时间
        // 线速度
        synced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale;
        synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale;
        synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale;
        // 角速度
        synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale;
        synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale;
        synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale;
        // 四元数插值有线性插值和球面插值,球面插值更准确,但是两个四元数差别不大是,二者精度相当
        // 由于是对相邻两时刻姿态插值,姿态差比较小,所以可以用线性插值
        synced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale;
        synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale;
        synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale;
        synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale;
        synced_data.orientation.Normlize();// 线性插值之后要归一化
        
        // 最后插入即可
        SyncedData.push_back(synced_data);
    
        return true;
    }
    }
    

    四种同步时间关系如下图所示。
    在这里插入图片描述

    二、evo里程计精度评价

    一共分两步:存数据和评价

    1. 数据存储

    我们在发布里程计的函数PublishData前面再加一个函数SaveTrajectory去存储数据就好。

    1、使用CreateDirectory和CreateFile创建文件夹和txt文件
    2、往文件里输入数据

    bool FrontEndFlow::SaveTrajectory() {
    
        // 1.创建文件夹和文件
        static std::ofstream ground_truth, laser_odom;
        static bool is_file_created = false;
        if (!is_file_created) {
            // 创建文件夹
            if (!FileManager::CreateDirectory(WORK_SPACE_PATH + "/slam_data/trajectory"))
                return false;
            // 创建txt文件ground_truth,laser_odom
            if (!FileManager::CreateFile(ground_truth, WORK_SPACE_PATH + "/slam_data/trajectory/ground_truth.txt"))
                return false;
            if (!FileManager::CreateFile(laser_odom, WORK_SPACE_PATH + "/slam_data/trajectory/laser_odom.txt"))
                return false;
            is_file_created = true;
        }
        // 2、往文件里输入数据
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 4; ++j) {
                ground_truth << gnss_odometry_(i, j);// gnss里程计
                laser_odom << laser_odometry_(i, j);// 激光雷达里程计
                // 一组数据发布成功到下一行,否则两个数据中间加空格" "
                if (i == 2 && j == 3) {
                    ground_truth << std::endl;
                    laser_odom << std::endl;
                } else {
                    ground_truth << " ";
                    laser_odom << " ";
                }
            }
        }
    
        return true;
    }
    
    

    2、EVO评价

    安装evo

    pip3 install evo --upgrade --no-binary evo

    evo_rpe 和 evo_ape ,前者评价的是每段距离内的误差,后者评价的是绝对误差随路程的累计。

    evo_rpe kitti ground_truth.txt laser_odom.txt -r trans_part --delta 100 --plot --plot_mode xyz
    

    评价每段距离内的误差:其中–delta 100表示的是每隔100米统计一次误差,这样统计的其实就是误差的百分比,和kitti的odometry榜单中的距离误差指标就可以直接对应了。

    evo_ape kitti ground_truth.txt laser_odom.txt -r full --plot --plot_mode xyz
    

    评价总累计误差:

    三、点云畸变补偿

    激光点数据不是瞬时获得的,激光测量时伴随着机器人的运动,一个数据采集周期内当激光雷达运动到不同位置时,机器人以为自己是在同一个位置测量出来的,所以会出现畸变。
    在这里插入图片描述
    激光雷达扫描过程中采集三维点云数据的过程在之前的博客《激光雷达坐标系、方向角和仰角
    点云数据如下图所示,以Velodyne公司的64线激光雷达HDL_64E为例

    • 64线表示64个发射接收机,每一时刻可以得到64个激光点,发射机的发射竖直角度在-24.8~~2度之间,竖直方向分辨率是0.4°
    • 一列64个发射接收机绕着竖直方向旋转,采集一圈得到4500列激光点,水平方向上分辨率是0.08°。
    • 每一个圆圈代表一个激光束旋转一周产生的数据

    在这里插入图片描述
    解决畸变首先计算采集过程中雷达的运动,然后在每帧激光点上补偿这个运动量。假设采集周期100ms内为匀速运动,可以采用旋转+平移补偿。

    程序设计

    增加的点云去除畸变操作只是在front_end_flow.cpp中的更新激光里程计函数UpdateLaserOdometry()中位姿估计之前进行了每一帧的点云去畸变操作。具体操作为:

        // imu系转激光雷达系
        current_velocity_data_.TransformCoordinate(imu_to_lidar_);
        // 每帧点云处理畸变:先获得运动信息,再坐标转化
        distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
        distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);
    

    第一点,激光雷达扫描过程中采集的数据中提供的速度是IMU所处位置的速度,我们需要将其转化到激光雷达所处位置的速度(线速度+角速度)。

    current_velocity_data_.TransformCoordinate(imu_to_lidar_);
    

    这里速度转换,角速度直接根据imu_to_lidar_变换矩阵相乘即可,线速度需要先旋转再加上叉乘结果。
    为此在include文件中专门定义了一个类VelocityData,里面包含线速度linear_velocity、角速度angular_velocity。以及类成员函数TransformCoordinate()

    imu-to-lidar两个坐标系转换主要是 根据imu_to_lidar的变换矩阵[R|t] 更新角速度 w 和线速度 v

    这里的w和v更新方式有点没看懂 ???

    w1=R * w0
    v1=R * v0 + w1^t (叉乘)

    Δv=w1×t=[0w2w1w20w0w1w00][t0t1t2]\Delta v =w_{1}\times t=\left [ \begin{matrix} 0 & -w2 & w1\\ w2 & 0&-w0 \\ -w1& w0 & 0 \end{matrix} \right ]\left [ \begin{matrix} t0\\ t1\\ t2 \end{matrix} \right ]

    外积

    1、外积的方向垂直于这两个向量。
    大小为|a| |b| sin<a,b>,是两个向量张成的四边形的面积。
    反对称矩阵 a^
    a×b=ijka1a2a3b1b2b3=[a2b3a3b2a3b1a1b3a1b2a2b1]=[0a3a2a30a1a2a10]b=aba\times b=\begin{Vmatrix} i &j & k\\ a1 & a2 & a3\\ b1& b2 & b3 \end{Vmatrix}=\begin{bmatrix} a2b3-a3b2\\ a3b1-a1b3\\ a1b2-a2b1 \end{bmatrix}=\begin{bmatrix} 0 & -a3 &a2 \\ a3 & 0 & -a1\\ -a2 &a1 & 0 \end{bmatrix}b=a^{\wedge }b
    2、外积还能表示两个向量的旋转。旋转向量:从a到b的旋转。
    3、变换矩阵的逆
    T1=[RTRTt01]T^{-1}=\begin{bmatrix} R^{T} & -R^{T}t\\ 0 & 1 \end{bmatrix}

    // imu转雷达坐标系:线速度、角速度
    void VelocityData::TransformCoordinate(Eigen::Matrix4f transform_matrix) {
        // 杆臂矩阵
        Eigen::Matrix4d matrix = transform_matrix.cast<double>();
        Eigen::Matrix3d t_R = matrix.block<3,3>(0,0);
    
        // 角速度更新
        Eigen::Vector3d w(angular_velocity.x, angular_velocity.y, angular_velocity.z);
        w = t_R * w;
    
        // 线速度先旋转
        Eigen::Vector3d v(linear_velocity.x, linear_velocity.y, linear_velocity.z);
        v = t_R * v;
        // 再加上叉乘结果,杆臂速度=角速度 叉乘 杆臂
        Eigen::Vector3d r(matrix(0,3), matrix(1,3), matrix(2,3));
        Eigen::Vector3d delta_v;
        delta_v(0) = w(1) * r(2) - w(2) * r(1);
        delta_v(1) = w(2) * r(0) - w(0) * r(2);
        delta_v(2) = w(1) * r(1) - w(1) * r(0);
        v = v + delta_v;
    
        angular_velocity.x = w(0);
        angular_velocity.y = w(1);
        angular_velocity.z = w(2);
        linear_velocity.x = v(0);
        linear_velocity.y = v(1);
        linear_velocity.z = v(2);
    }
    

    将线速度和角速度从IMU系转化到激光雷达坐标系之后,需要进行点云去畸变操作。为此在models文件夹下专门建立一个文件夹scan_adjust,里面包含两个重要函数:获得运动信息SetMotionInfo()和去畸变AdjustCloud()

    // 每帧点云处理畸变:先获得运动信息,再坐标转化
        distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
        distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);
    

    去畸变scan_adjust

    1.获取运动信息SetMotionInfo()

    获取载体运动信息,角速度angular_rate_ 、速度velocity_分别用来计算相对角度和相对位移。

        // 获得运动信息:线速度、角速度
    void DistortionAdjust::SetMotionInfo(float scan_period, VelocityData velocity_data) {
        scan_period_ = scan_period;
        velocity_ << velocity_data.linear_velocity.x, velocity_data.linear_velocity.y, velocity_data.linear_velocity.z;
        angular_rate_ << velocity_data.angular_velocity.x, velocity_data.angular_velocity.y, velocity_data.angular_velocity.z;
    }
    

    2.去除畸变AdjustCloud()

    在已知载体运动信息,角速度和线速度之后,接下来要对每个扫描周期内激光点云进行补偿。载体运动过程中,每一列激光点的基准坐标系不同,所以我们需要在得到每个激光束接收时刻后,将激光点云转换到相对与初始时刻的相对运动即可,让此时激光点坐标乘上这个相对转换,就转换成了在初始坐标系下的激光点。
    函数输入原始点云,输出去除畸变的点云。

    bool DistortionAdjust::AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr)
    

    0、预处理
    初始化输入点云和输出点云。

     // 输入、输出点云
        CloudData::CLOUD_PTR origin_cloud_ptr(new CloudData::CLOUD(*input_cloud_ptr));// 原始点云是输入点云
        output_cloud_ptr->points.clear();
    
        float orientation_space = 2.0 * M_PI;// 方向角一圈 2*pi
        float delete_space = 5.0 * M_PI / 180.0; // 过滤正负5度激光点
    

    1、初始点云先旋转
    通过atan2(y/x)第一个激光点的方位角,并以Z轴为旋转角,旋转弧度为方位角,得到旋转矩阵rotate_matrix
    对初始点云进行旋转pcl::transformPointCloud
    更新线速度、角速度(旋转)

        // 1.初始点云先旋转
        // 起始点旋转矩阵
        float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x);// 起始点方位角(欧拉角) atan2(y/x)
        Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ()); // 旋转向量,以Z轴为旋转轴,旋转方位角弧度
        Eigen::Matrix3f rotate_matrix = t_V.matrix();// 旋转矩阵
        Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();// 变换矩阵
        transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();
        // 原始点云旋转过后
        pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);// 输入、输出、变换矩阵
    
        // 线速度、角速度
        velocity_ = rotate_matrix * velocity_;
        angular_rate_ = rotate_matrix * angular_rate_;
    

    2、后续点云去除畸变

    • 获得每个激光点的方位角orientation并预处理
    • 根据顺序扫描时间real_time对点云进行去畸变处理(旋转+平移)
    • 点云去除畸变后旋转pcl::transformPointCloud
        for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {
            // 2.1 获得方位角及预处理
            float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x);// 顺序扫描角度,方位角
            
            if (orientation < 0.0)// 保证角度都是正的
                orientation += 2.0 * M_PI;
            
            if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)// 过滤了+-5度的点
                continue;
    
            // 2.2 根据顺序扫描时间对点云进行去畸变处理(旋转+平移)
            // 减去scan_period/2,把点云转换到该帧采集的中间时刻上去
            // bag里点云的时刻是该帧点云起始时刻和终止时刻的平均值
            float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0;
    
            Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x,
                                         origin_cloud_ptr->points[point_index].y,
                                         origin_cloud_ptr->points[point_index].z);
             
            Eigen::Matrix3f current_matrix = UpdateMatrix(real_time);// 根据运行时间获得当前旋转矩阵
            Eigen::Vector3f rotated_point = current_matrix * origin_point;// 旋转矩阵*三维点
            Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time;// 旋转+平移
    
            CloudData::POINT point;
            point.x = adjusted_point(0);
            point.y = adjusted_point(1);
            point.z = adjusted_point(2);
            output_cloud_ptr->points.push_back(point);
        }
        // 2.3 点云去除畸变后旋转
        pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());
        return true;
    }
    

    获得当前旋转矩阵UpdateMatrix()
    由顺序扫描时间和角速度 获得当前扫描的角度,然后通过旋转向量Z-Y-X,最后转化为旋转矩阵。

    // 已知运行时间获得旋转矩阵
    Eigen::Matrix3f DistortionAdjust::UpdateMatrix(float real_time) {
        // 角度=角速度*运行时间
        Eigen::Vector3f angle = angular_rate_ * real_time;
        // 旋转向量z-y-x
        Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ());
        Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf t_V;
        t_V = t_Vz * t_Vy * t_Vx;
        // 转为旋转矩阵
        return t_V.matrix();
    }
    

    注意:点云的整体旋转。第一次转因为:得到的变换矩阵是雷达相对于世界坐标系的,所以要逆。

         // 1、要根据第一个点获得的变换矩阵,把原始点云旋转到相对于世界坐标系。
         transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();// 这里为什么要用逆 ???
        // 原始点云旋转过后
        pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);// 输入、输出、变换矩阵
        // 2、后续点云去除畸变
        ...
        // 3、 点云去除畸变后再把点云旋转回去。
        pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());
        return true;
    

    Eigen库中各种形式的表示如下:

    旋转矩阵(3X3):Eigen::Matrix3d
    旋转向量(3X1):Eigen::AngleAxisd
    四元数(4X1):Eigen::Quaterniond
    平移向量(3X1):Eigen::Vector3d
    变换矩阵(4X4):Eigen::Isometry3d
    

    仿射变换Affine3f进行点云变换。

    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
     
      // 在 X 轴上定义一个 2.5 米的平移.
      transform_2.translation() << 2.5, 0.0, 0.0;
      // 和前面一样的旋转; Z 轴上旋转 theta 弧度
      transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
    
      // 执行变换,并将结果保存在新创建的‎‎ transformed_cloud ‎‎中
      pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      // pcl库实现点云变化
      pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
    
    

    获得AngleAxisd旋转向量

    //1.使用旋转的角度和旋转轴向量(此向量为单位向量)来初始化角轴
        AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以(0,0,1)为旋转轴,旋转45度
        cout << "Rotation_vector1" << endl << V1.matrix() << endl;
    

    参考:
    https://zhuanlan.zhihu.com/p/109379384

    展开全文
  • ROS中的多传感器时间同步

    千次阅读 2019-06-11 19:30:42
    在对多传感器数据融合时,由于各个传感器采集数据的发布频率的不同,例如odom一般为50Hz、imu一般为100Hz、camera 一般为25Hz,需要将传感器数据进行时间同步后才能进行融合。 方法 分别订阅不同的需要融合的...

    问题

    在对多传感器数据融合时,由于各个传感器采集数据的发布频率的不同,例如odom一般为50Hz、imu一般为100Hz、camera 一般为25Hz,需要将传感器数据进行时间同步后才能进行融合。

    方法

    分别订阅不同的需要融合的传感器的主题,通过TimeSynchronizer 统一接收多个主题,并产生一个同步结果的回调函数,在回调函数里处理同步时间后的数据。

    输入

    • C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr<M const>&). The number of filters supported is determined by the number of template arguments the class was created with.

    • Python: N separate filters, each of which has signature callback(msg).

    输出

    • C++: For message types M0..M8, void callback(const boost::shared_ptr<M0 const>&, ..., const boost::shared_ptr<M8 const>&). The number of parameters is determined by the number of template arguments the class was created with.

    • Python: callback(msg0.. msgN). The number of parameters is determined by the number of template arguments the class was created with.

    示例

    #include <message_filters/subscriber.h>
    #include <message_filters/time_synchronizer.h>
    #include <sensor_msgs/Image.h>
    #include <sensor_msgs/CameraInfo.h>
    
    using namespace sensor_msgs;
    using namespace message_filters;
    
    void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
    {
      // Solve all of perception here...
    }
    
    int main(int argc, char** argv)
    {
      ros::init(argc, argv, "vision_node");
    
      ros::NodeHandle nh;
    
      message_filters::Subscriber<Image> image_sub(nh, "image", 1);
      message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
      TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
      sync.registerCallback(boost::bind(&callback, _1, _2));
    
      ros::spin();
    
      return 0;
    }

    注意

    1. 在示例中,只有当image_sub与info_sub都接收到话题的数据时,才会执行回调函数calback;
    2. 即使当"image"与"camera_info"两个话题发布频率一致时,也不能保证回调函数callback的频率和他们一样,实际上会低很多。

    更多

    http://wiki.ros.org/message_filters

    展开全文
  • 传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。 2、融合的原理: An example is the time synchronizer, ...

    1、存在的问题

    多传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。

    2、融合的原理:

    An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.
    分别订阅不同的需要融合的传感器的主题,通过TimeSynchronizer 统一接收多个主题,只有在所有的topic都有相同的时间戳时,才会产生一个同步结果的回调函数,在回调函数里处理同步时间后的数据。

    注意
    只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了,则整个回调函数永远无法触发回调。
    当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率,一般会很低。实际测试订阅两个需要同步的主题,odom 50Hz、imu 50Hz,而回调函数的频率只有24Hz左右。
    

    3、具体实现

    方式一: 全局变量形式 : TimeSynchronizer

    步骤:

    1. message_filter ::subscriber 分别订阅不同的输入topic

    2. TimeSynchronizer<Image,CameraInfo> 定义时间同步器;

    3. sync.registerCallback 同步回调

    4. void callback(const ImageConstPtr&image, const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数

    相应的API message_filters::TimeSynchronizer

    //wiki参考demo http://wiki.ros.org/message_filters
    #include <message_filters/subscriber.h>
    #include <message_filters/time_synchronizer.h>
    #include <sensor_msgs/Image.h>
    #include <sensor_msgs/CameraInfo.h>
    
    using namespace sensor_msgs;
    using namespace message_filters;
    
    void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回调中包含多个消息
    {
      // Solve all of perception here...
    }
    
    int main(int argc, char** argv)
    {
      ros::init(argc, argv, "vision_node");
    
      ros::NodeHandle nh;
    
      message_filters::Subscriber<Image> image_sub(nh, "image", 1);             // topic1 输入
      message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);   // topic2 输入
      TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);       // 同步
      sync.registerCallback(boost::bind(&callback, _1, _2));                   // 回调
    
      ros::spin();
    
      return 0;
    }
    //
    

    参考连接:http://wiki.ros.org/message_filters

    方式二: 类成员的形式 message_filters::Synchronizer

    说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.

    1. 头文件
    #include <message_filters/subscriber.h>
    #include <message_filters/synchronizer.h>
    #include <message_filters/sync_policies/approximate_time.h>
    
    1. 定义消息同步机制
    typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;
    
    1. 定义类成员变量
    message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ;             // topic1 输入
    message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入
    message_filters::Synchronizer<slamSyncPolicy>* sync_;
    

    4.类构造函数中开辟空间new

    odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);
    img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);
       
    sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_);
    sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));
    
    1. 类成员函数回调处理
    void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg)  //回调中包含多个消息
    {
        //TODO
        fStampAll<<pOdom->header.stamp<<"    "<<pImg->header.stamp<<endl;
        getOdomData(pOdom);                   //
        is_img_update_ = getImgData(pImg);    // 像素值
        cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
             << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
        fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
              << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
        pixDataToMetricData();
        static bool FINISH_INIT_ODOM_STATIC = false;
        if(FINISH_INIT_ODOM_STATIC)
        {
            ekfslam(robot_odom_);
        }
        else if(is_img_update_)
        {
            if(addInitVectorFull())
            {
                computerCoordinate();
                FINISH_INIT_ODOM_STATIC = true;
            }
        }
    }
    

    参考链接

    https://blog.csdn.net/xingdou520/article/details/83783768
    https://blog.csdn.net/zyh821351004/article/details/47758433

    展开全文
  • 传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。 融合的原理: 分别订阅不同的需要融合的传感器的主题,通过...

    1、解决的问题

    多传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。

    融合的原理:

    分别订阅不同的需要融合的传感器的主题,通过TimeSynchronizer 统一接收多个主题,并产生一个同步结果的回调函数,在回调函数里处理同步时间后的数据。

    注意

    1. 只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了,则整个回调函数永远无法触发回调。
    2. 当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率,一般会很低。实际测试订阅两个需要同步的主题,odom 50Hz、imu 50Hz,而回调函数的频率只有24Hz左右。

    2、原文描述

    Time Synchronizer
    See also: C++ message_filters::TimeSynchronizer API docs, Python message_filters.TimeSynchronizer

    The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

    Connections
    Input

    C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr&). The number of filters supported is determined by the number of template arguments the class was created with. Python: N separate filters, each of which has signature callback(msg).
    Output

    C++: For message types M0…M8, void callback(const boost::shared_ptr&, …, const boost::shared_ptr&). The number of parameters is determined by the number of template arguments the class was created with. Python: callback(msg0… msgN). The number of parameters is determined by the number of template arguments the class was created with.
    Example (C++)
    Suppose you are writing a ROS node that needs to process data from two time synchronized topics. Your program will probably look something like this:

    #include <message_filters/subscriber.h>
    #include <message_filters/time_synchronizer.h>
    #include <sensor_msgs/Image.h>
    #include <sensor_msgs/CameraInfo.h>
    
    using namespace sensor_msgs;
    using namespace message_filters;
    
    void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
    {
      // Solve all of perception here...
    }
    
    int main(int argc, char** argv)
    {
      ros::init(argc, argv, "vision_node");
    
      ros::NodeHandle nh;
    
      message_filters::Subscriber<Image> image_sub(nh, "image", 1);
      message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
      TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
      sync.registerCallback(boost::bind(&callback, _1, _2));
    
      ros::spin();
    
      return 0;
    }
    

    参考

    http://wiki.ros.org/message_filters
    https://blog.csdn.net/lewif/article/details/80136401

    展开全文
  • ros多传感器时间同步(message_filters)

    千次阅读 2020-05-25 00:37:21
    举个例子,比如时间同步器,它接收来自多个源的不同类型的消息,并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们,也就是起到了一个消息同步输出的效果。 下面同步雷达和图像的数据.代码如下: // msg...
  • 分析其中的源码,发现,它是将点云图和RGB图进行了时间的同步,点云提供了... 所以,在这里我所感兴趣的是时间同步问题,之前做传感器融合的自动驾驶算法时,基于卡尔曼滤波融合激光雷达和毫米波雷达两种传感器信息...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 473
精华内容 189
关键字:

传感器时间同步