資源簡介
1去除激光雷達運動畸變 2結合ROS庫,pcl庫 3包含2個ros包
代碼片段和文件信息
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
#include?
pcl::visualization::CloudViewer?g_PointCloudView(“PointCloud?View“);
class?LidarMotionCalibrator
{
public:
????LidarMotionCalibrator(tf::TransformListener*?tf)
????{
????????tf_?=?tf;
????????scan_sub_?=?nh_.subscribe(“scan“?10?&LidarMotionCalibrator::ScanCallBack?this);
????}
????~LidarMotionCalibrator()
????{
????????if(tf_!=NULL)
????????????delete?tf_;
????}
????//?拿到原始的激光數據來進行處理
????void?ScanCallBack(const?sensor_msgs::LaserScanConstPtr&?scan_msg)
????{
????????//轉換到矯正需要的數據
????????ros::Time?startTime?endTime;
????????startTime?=?scan_msg->header.stamp;
????????sensor_msgs::LaserScan?laserScanMsg?=?*scan_msg;
????????//得到最終點的時間
????????int?beamNum?=?laserScanMsg.ranges.size();
????????endTime?=?startTime?+?ros::Duration(laserScanMsg.time_increment?*?beamNum);
????????//?將數據復制出來
????????std::vector?anglesranges;
????????for(int?i?=?0;?i?????????{
????????????double?lidar_dist?=?laserScanMsg.ranges[i];
????????????double?lidar_angle?=?laserScanMsg.angle_min?+?laserScanMsg.angle_increment?*?i;
????????????ranges.push_back(lidar_dist);
????????????angles.push_back(lidar_angle);
????????}
????????//轉換為pcl::pointcloud?for?visuailization
????????visual_cloud_.clear();
????????for(int?i?=?0;?i?????????{
????????????if(ranges[i]?0.05?||?std::isnan(ranges[i])?||?std::isinf(ranges[i]))
????????????????continue;
????????????pcl::PointXYZRGB?pt;
????????????pt.x?=?ranges[i]?*?cos(angles[i]);
????????????pt.y?=?ranges[i]?*?sin(angles[i]);
????????????pt.z?=?1.0;
????????????//?pack?r/g/b?into?rgb
????????????unsigned?char?r?=?255?g?=?0?b?=?0;????//red?color
????????????unsigned?int?rgb?=?((unsigned?int)r?<16?|?(unsigned?int)g?<8?|?(unsigned?int)b);
????????????pt.rgb?=?*reinterpret_cast(&rgb);
????????????visual_cloud_.push_back(pt);
????????}
????????std::cout?<
????????//進行矯正
????????Lidar_Calibration(rangesangles
??????????????????????????startTime
??????????????????????????endTime
??????????????????????????tf_);
????????//轉換為pcl::pointcloud?for?visuailization
????????for(int?i?=?0;?i?????????{
????????????if(ranges[i]?0.05?||?std::isnan(ranges[i])?||?std::isinf(ranges[i]))
????????????????continue;
????????????pcl::PointXYZRGB?pt;
????????????pt.x?=?ranges[i]?*?cos(angles[i]);
????????????pt.y?=?ranges[i]?*?sin(angles[i]);
????????????pt.z?=?1.0;
????????????unsigned?char?r?=?0?g?=?255?b?=?0;????//?blue?color
????????????unsigned?int?rgb?=?((unsigned?int)r?<16?|?(unsigned?int)g?<8?|?(unsigned?int)b);
????????????pt.rgb?=?*reinterpret_cast(&rgb);
????????????visual_cloud_
- 上一篇:SVM_Iris.rar
- 下一篇:ARM匯編實現矩陣轉置
評論
共有 條評論