velodyne16:点云畸变去除（源码）

最新推荐文章于 2024-04-10 10:40:48 发布

Unite One

最新推荐文章于 2024-04-10 10:40:48 发布

阅读量2.8k

点赞数 5

分类专栏： pcl 902 文章标签：自动驾驶

本文链接：https://blog.csdn.net/xx970829/article/details/122185239

版权

902 同时被 2 个专栏收录

31 篇文章 1 订阅

订阅专栏

pcl

7 篇文章 3 订阅

订阅专栏

说明

点云畸变主要是由运动导致的，velodyne16为10hz，如果运动较快则不得不去除畸变。
去除畸变时最核心的是得到每个点的具体扫描时间，loam主要通过计算角度，lio-sam主要通过水平分辨率展开，出于好奇，自己写了一个程序，主要通过每个扫描点的自带的时间[注意：扫描点的自带的时间并不是每次都从0开始的，其内部的微秒计时器会漂移]

源码

 //*************************************************************************************************
//    点云畸变去除 , 订阅： points_raw
//                 发布： 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.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  "<<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;
   // 防止第一帧点云无位姿索引
   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;
    // 取出激光点云队列中最早的一帧
    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(-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);  // 对应 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;
}