郑州哪家网站建设好,c2c模式流程图,网站建设通报,cute wordpress主题实现方式#xff1a; 模仿源代码#xff0c;在cartographer_ros写一个函数#xff0c;以函数指针的方式传入cartographer后端#xff0c;然后接收矫正后的scan数据#xff0c;然后按照话题laserScan发布出来。 需要同时发布点云强度信息的#xff0c;还要自己添加含有强度…实现方式 模仿源代码在cartographer_ros写一个函数以函数指针的方式传入cartographer后端然后接收矫正后的scan数据然后按照话题laserScan发布出来。 需要同时发布点云强度信息的还要自己添加含有强度信息的代码。 核心转换代码如下 for (size_t i 0; i synchronized_data.ranges.size(); i) {const sensor::TimedRangefinderPoint hit synchronized_data.ranges[i].point_time;const Eigen::Vector3f origin_in_local range_data_poses[i] *synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);sensor::RangefinderPoint hit_in_local range_data_poses[i] * sensor::ToRangefinderPoint(hit);const Eigen::Vector3f delta hit_in_local.position - origin_in_local;const float range delta.norm();if (range options_.min_range()) {if (range options_.max_range()) {hit_in_local.intensity synchronized_data.ranges[i].intensity; //accumulated_range_data_.returns.push_back(hit_in_local);//transform::Rigid3f sensor_to_tracking transform::Rigid3f::Translation(Eigen::Vector3f(synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[0],synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[1],synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[2]));undistortion_scan_data_.returns.push_back(sensor_to_tracking.inverse() *(range_data_poses[0].inverse() * hit_in_local));} else {hit_in_local.position origin_in_local options_.missing_data_ray_length() / range * delta;hit_in_local.intensity synchronized_data.ranges[i].intensity; // accumulated_range_data_.misses.push_back(hit_in_local);//transform::Rigid3f sensor_to_tracking transform::Rigid3f::Translation(Eigen::Vector3f(synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[0],synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[1],synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[2]));undistortion_scan_data_.returns.push_back(sensor_to_tracking.inverse() *(range_data_poses[0].inverse() * hit_in_local));}}else{//hit_in_local.position Eigen::Vector3f::Zero();//origin_in_local 0.0 / range * delta;hit_in_local.intensity 0; // //accumulated_range_data_.misses.push_back(hit_in_local);//undistortion_scan_data_.returns.push_back(hit_in_local);}}其他改的地方其实挺多因为 1.我把强度信息也传进去了。 2.对于注册返回的函数指针为了不破坏原有的代码结构都是自己添加新的接口。 3.因为源代码对于一些异常点进行了移除而我要保留所有矫正后的点所以也修改了部分代码。 4.最核心的就是数据转换因为carto处理后的数据的坐标系是local的所以要要逆变换回来。因为carto传入的数据考虑了激光头的外参数所以传回来也要外参数逆变换回来。
好了大家可以开心的使用畸变矫正后的scan话题了。
