目錄

• 說(shuō)明
• • 源碼
  • • 效果

說(shuō)明

點(diǎn)云畸變主要是由運(yùn)動(dòng)導(dǎo)致的，velodyne16為10hz，如果運(yùn)動(dòng)較快則不得不去除畸變。
去除畸變時(shí)最核心的是得到每個(gè)點(diǎn)的具體掃描時(shí)間，loam主要通過(guò)計(jì)算角度，lio-sam主要通過(guò)水平分辨率展開，出于好奇，自己寫了一個(gè)程序，主要通過(guò)每個(gè)掃描點(diǎn)的自帶的時(shí)間[注意：掃描點(diǎn)的自帶的時(shí)間并不是每次都從0開始的，其內(nèi)部的微秒計(jì)時(shí)器會(huì)漂移]

源碼

//************************************************************************************************* // 點(diǎn)云畸變?nèi)コ?, 訂閱： points_raw // 發(fā)布： map/cloud_deskewed // // source ~/catkin_gap/devel/setup.bash && rosrun my_cloud_map cloud_distortion // //****************************************************************************************************#include <ros/ros.h> #include <mutex> #include <iomanip>#include <pcl_ros/point_cloud.h> #include <pcl_ros/transforms.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h>#include <sensor_msgs/PointCloud2.h> #include <geometry_msgs/PoseStamped.h>#define PI 3.1415926using namespace std; using namespace pcl;struct VelodynePointXYZIRT {PCL_ADD_POINT4DPCL_ADD_INTENSITY;uint16_t ring;float time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW } EIGEN_ALIGN16; POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)(uint16_t, ring, ring) (float, time, time) )struct eulerTranslation {double euler_x;double euler_y;double euler_z;double translation_x;double translation_y;double translation_z;double time;};using PointXYZIRT = VelodynePointXYZIRT;class cloud_distortion { public:cloud_distortion(){cout<< "cloud_distortion "<<endl;sub_cloud = n.subscribe<sensor_msgs::PointCloud2>("map/points_raw", 100, &cloud_distortion::cloudCallback,this);sub_pose = n.subscribe<geometry_msgs::PoseStamped>("map/row_pos", 1000, &cloud_distortion::poseCallback,this);pub_cloud = n.advertise<sensor_msgs::PointCloud2> ("map/cloud_deskewed", 1);}void cloudCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg );void poseCallback (const boost::shared_ptr<const geometry_msgs::PoseStamped>& posmsg );bool cloudQueueAdd(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg );bool obtainPose();void cloudDistortionRemoval();PointXYZIRT pointDistortionRemoval(PointXYZIRT currentPoint);bool currentPointEulerTrans(double currentPointTime );void publisherCloud();private:ros::NodeHandle n;ros::Subscriber sub_cloud;ros::Subscriber sub_pose;ros::Publisher pub_cloud;std::mutex poseLock;std::deque<geometry_msgs::PoseStamped> poseQueue;std::deque<sensor_msgs::PointCloud2> cloudQueue;pcl::PointCloud<PointXYZIRT>::Ptr currentCloudIn;pcl::PointCloud<PointXYZIRT>::Ptr currentCloudOut;ros::Time time;// 防止第一幀點(diǎn)云無(wú)位姿索引bool firstBeginAddCloud=true;bool beginAddCloud=false;double curentTimeDiff;double currentCloudtime;double currentCloudtimeEnd;// 為原始位姿（map下）eulerTranslation currentPointEulerTranslation;vector < eulerTranslation > currentEulerTransVec;int k=0;};void cloud_distortion::cloudCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg ) {currentCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());currentCloudOut.reset(new pcl::PointCloud<PointXYZIRT>());if(!cloudQueueAdd(cloudmsg))return;if(!obtainPose())return;cloudDistortionRemoval();publisherCloud(); } void cloud_distortion::poseCallback(const boost::shared_ptr<const geometry_msgs::PoseStamped>& posmsg ) {std::lock_guard<std::mutex> lock1(poseLock);poseQueue.push_back(*posmsg);if(firstBeginAddCloud){beginAddCloud=true;firstBeginAddCloud=false;} }bool cloud_distortion::cloudQueueAdd(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg ) {if(!beginAddCloud)return false;cloudQueue.push_back(*cloudmsg);if (cloudQueue.size() <= 2)return false;// 取出激光點(diǎn)云隊(duì)列中最早的一幀sensor_msgs::PointCloud2 currentCloudMsg;currentCloudMsg = std::move(cloudQueue.front());cloudQueue.pop_front();pcl::moveFromROSMsg(currentCloudMsg, *currentCloudIn);time = currentCloudMsg.header.stamp;currentCloudtime= currentCloudMsg.header.stamp.toSec();double firstTime=currentCloudIn->points[0].time;double endTime=currentCloudIn->points.back().time;curentTimeDiff = endTime-firstTime;currentCloudtimeEnd = currentCloudtime + curentTimeDiff;return true; }bool cloud_distortion::obtainPose() {// 從pose隊(duì)列中刪除當(dāng)前激光幀0.05s前面時(shí)刻的imu數(shù)據(jù)while (!poseQueue.empty()){if (poseQueue.front().header.stamp.toSec() < currentCloudtime - 0.05)poseQueue.pop_front();elsebreak;}if (poseQueue.empty())return false;currentEulerTransVec.clear();int poseBegin=-1 , poseEnd=-1;for (std::size_t i = 0; i < poseQueue.size()-1; i++){double currentPosTime = poseQueue[i].header.stamp.toSec();double nextPosTime = poseQueue[i+1].header.stamp.toSec();if(currentPosTime<= currentCloudtime && currentCloudtime< nextPosTime)poseBegin=i;if(currentPosTime<= currentCloudtimeEnd && currentCloudtimeEnd< nextPosTime){poseEnd=i+1;break;}}if((poseBegin<0)||(poseEnd<0))return false;for (int i = poseBegin; i <= poseEnd; i++){geometry_msgs::PoseStamped thisPosMsg = poseQueue[i];double currentPosTime = thisPosMsg.header.stamp.toSec();// 四元數(shù)轉(zhuǎn)換為eulerEigen::Quaterniond quaternion( thisPosMsg.pose.orientation.w,thisPosMsg.pose.orientation.x,thisPosMsg.pose.orientation.y,thisPosMsg.pose.orientation.z );Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);eulerTranslation currentEulerTrans;currentEulerTrans.euler_x=eulerAngle(2);//xcurrentEulerTrans.euler_y=eulerAngle(1);currentEulerTrans.euler_z=eulerAngle(0);currentEulerTrans.translation_x=thisPosMsg.pose.position.x;currentEulerTrans.translation_y=thisPosMsg.pose.position.y;currentEulerTrans.translation_z=thisPosMsg.pose.position.z;currentEulerTrans.time=currentPosTime;currentEulerTransVec.push_back( currentEulerTrans );}return true; }void cloud_distortion::cloudDistortionRemoval() {for(std::size_t i=0;i< currentCloudIn->size();i++ ){if(-60<currentCloudIn->points[i].y && currentCloudIn->points[i].y<60 && (-2>currentCloudIn->points[i].x || currentCloudIn->points[i].x>2) && currentCloudIn->points[i].x<90){PointXYZIRT currentPoint;currentPoint.x=currentCloudIn->points[i].x;currentPoint.y=currentCloudIn->points[i].y;currentPoint.z=currentCloudIn->points[i].z;currentPoint.intensity=currentCloudIn->points[i].intensity;currentPoint.ring=currentCloudIn->points[i].ring;currentPoint.time= currentCloudIn->points[i].time-currentCloudIn->points[0].time;PointXYZIRT newCurrentPoint;newCurrentPoint=pointDistortionRemoval(currentPoint);currentCloudOut->push_back(newCurrentPoint);}} }PointXYZIRT cloud_distortion::pointDistortionRemoval(PointXYZIRT currentPoint) {if(!currentPointEulerTrans( currentPoint.time ))return currentPoint;Eigen::Vector3d euler(currentPointEulerTranslation.euler_z,currentPointEulerTranslation.euler_y,currentPointEulerTranslation.euler_x); // 對(duì)應(yīng) z y xEigen::Matrix3d rotation_matrix;rotation_matrix = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitX());Eigen::Matrix4d rotaTrans;for(int i=0 ;i<3 ;i++){rotaTrans(0,i)= rotation_matrix(0,i);rotaTrans(1,i)= rotation_matrix(1,i);rotaTrans(2,i)= rotation_matrix(2,i);rotaTrans(3,i)= 0;}rotaTrans(0,3)= currentPointEulerTranslation.translation_x;rotaTrans(1,3)= currentPointEulerTranslation.translation_y;rotaTrans(2,3)= currentPointEulerTranslation.translation_z;rotaTrans(3,3)= 1;Eigen::MatrixXd oriPoint(4,1);oriPoint(0,0)= currentPoint.x;oriPoint(1,0)= currentPoint.y;oriPoint(2,0)= currentPoint.z;oriPoint(3,0)= 1;//去畸變Eigen::MatrixXd newPoint=rotaTrans*oriPoint; //lidartoimu_niPointXYZIRT newCurrentPoint;newCurrentPoint.x = newPoint(0,0);newCurrentPoint.y = newPoint(1,0);newCurrentPoint.z = newPoint(2,0);newCurrentPoint.intensity= currentPoint.intensity;newCurrentPoint.ring = currentPoint.ring;newCurrentPoint.time = currentPoint.time;return newCurrentPoint; }bool cloud_distortion::currentPointEulerTrans(double currentPointTime ) {if (currentEulerTransVec.empty())return false;double thisPointTime= currentCloudtime + currentPointTime;eulerTranslation currentPointEulerTransBegin;eulerTranslation currentPointEulerTransEnd;for(std::size_t i=0 ;i < currentEulerTransVec.size() ;i++){if(currentEulerTransVec[i].time <= thisPointTime )currentPointEulerTransBegin = currentEulerTransVec[i];else{currentPointEulerTransEnd = currentEulerTransVec[i];break;}}double ac= thisPointTime-currentPointEulerTransBegin.time;double ab= currentPointEulerTransEnd.time-currentPointEulerTransBegin.time;double timeRatio = ac/ab; //占比currentPointEulerTranslation.euler_x = (currentPointEulerTransEnd.euler_x -currentPointEulerTransBegin.euler_x) *timeRatio;currentPointEulerTranslation.euler_y = (currentPointEulerTransEnd.euler_y -currentPointEulerTransBegin.euler_y) *timeRatio;currentPointEulerTranslation.euler_z = (currentPointEulerTransEnd.euler_z -currentPointEulerTransBegin.euler_z) *timeRatio;currentPointEulerTranslation.translation_x = (currentPointEulerTransEnd.translation_x-currentPointEulerTransBegin.translation_x)*timeRatio;currentPointEulerTranslation.translation_y = (currentPointEulerTransEnd.translation_y-currentPointEulerTransBegin.translation_y)*timeRatio;currentPointEulerTranslation.translation_z = (currentPointEulerTransEnd.translation_z-currentPointEulerTransBegin.translation_z)*timeRatio;//currentPointEulerTranslation.time = thisPointTime - currentPointEulerTransBegin.time;return true; }void cloud_distortion::publisherCloud() {//發(fā)布點(diǎn)云sensor_msgs::PointCloud2 output;pcl::toROSMsg(*currentCloudOut, output);output.header.stamp = time;output.header.frame_id = "map";pub_cloud.publish(output); }int main(int argc, char **argv) {ros::init(argc, argv, "cloud_distortion");cloud_distortion cd;ros::spin();return 0; }

效果

圖中白色點(diǎn)為原始點(diǎn)云，彩色為去畸變后的點(diǎn)云，可以看出去畸變前點(diǎn)云具有一定滯后（應(yīng)該是無(wú)人機(jī)向前飛行造成的）

總結(jié)

以上是生活随笔為你收集整理的velodyne16:点云畸变去除（源码）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。