velodyne16:点云畸变去除（源码）
目录
- 说明
-
- 源码
-
- 效果
说明
点云畸变主要是由运动导致的，velodyne16为10hz，如果运动较快则不得不去除畸变。
去除畸变时最核心的是得到每个点的具体扫描时间，loam主要通过计算角度，lio-sam主要通过水平分辨率展开，出于好奇，自己写了一个程序，主要通过每个扫描点的自带的时间[注意：扫描点的自带的时间并不是每次都从0开始的，其内部的微秒计时器会漂移]
源码
//*************************************************************************************************
// 点云畸变去除 , 订阅： points_raw
// 发布： map/cloud_deskewed
//
// source ~/catkin_gap/devel/setup.bash && rosrun my_cloud_map cloud_distortion
//
//****************************************************************************************************
#include
#include
#include
#include
#include
#include
#include
#include
#include
#define PI 3.1415926
using namespace std;
using namespace pcl;
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_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 "<("map/points_raw", 100, &cloud_distortion::cloudCallback,this);
sub_pose = n.subscribe("map/row_pos", 1000, &cloud_distortion::poseCallback,this);
pub_cloud = n.advertise ("map/cloud_deskewed", 1);
}
void cloudCallback(const boost::shared_ptr& cloudmsg );
void poseCallback (const boost::shared_ptr& posmsg );
bool cloudQueueAdd(const boost::shared_ptr& 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 poseQueue;
std::deque cloudQueue;
pcl::PointCloud::Ptr currentCloudIn;
pcl::PointCloud::Ptr currentCloudOut;
ros::Time time;
// 防止第一帧点云无位姿索引
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& cloudmsg )
{
currentCloudIn.reset(new pcl::PointCloud());
currentCloudOut.reset(new pcl::PointCloud());
if(!cloudQueueAdd(cloudmsg))
return;
if(!obtainPose())
return;
cloudDistortionRemoval();
publisherCloud();
}
void cloud_distortion::poseCallback(const boost::shared_ptr& posmsg )
{
std::lock_guard lock1(poseLock);
poseQueue.push_back(*posmsg);
if(firstBeginAddCloud)
{
beginAddCloud=true;
firstBeginAddCloud=false;
}
}
bool cloud_distortion::cloudQueueAdd(const boost::shared_ptr& cloudmsg )
{
if(!beginAddCloud)
return false;
cloudQueue.push_back(*cloudmsg);
if (cloudQueue.size() <= 2)
return false;
// 取出激光点云队列中最早的一帧
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队列中删除当前激光帧0.05s前面时刻的imu数据
while (!poseQueue.empty())
{
if (poseQueue.front().header.stamp.toSec() < currentCloudtime - 0.05)
poseQueue.pop_front();
else
break;
}
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();
// 四元数转换为euler
Eigen::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);//x
currentEulerTrans.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(-60points[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); // 对应 z y x
Eigen::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_ni
PointXYZIRT 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()
{
//发布点云
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;
}
效果
图中白色点为原始点云，彩色为去畸变后的点云，可以看出去畸变前点云具有一定滞后（应该是无人机向前飞行造成的）