velodyne16:点云畸变去除（源码）
2024-05-19 22:27:20
目录
- 说明
- 源码
- 效果
说明
点云畸变主要是由运动导致的，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.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;// 防止第一帧点云无位姿索引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();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();// 四元数转换为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); // 对应 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()
{//发布点云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;
}效果
图中白色点为原始点云，彩色为去畸变后的点云，可以看出去畸变前点云具有一定滞后（应该是无人机向前飞行造成的）
velodyne16:点云畸变去除（源码）相关推荐
- php蓝奏云解析源码,PHP获取蓝奏云直链解析源码
PHP获取蓝奏云直链的源码案例 可以用来做api调用到网站做下载 调用方法: 无密码:http://www.yyob.com/lanzou/?url=https://www.lanzous.com/i ...
- 基于Java毕业设计在线云音乐系统源码+系统+mysql+lw文档+部署软件
基于Java毕业设计在线云音乐系统源码+系统+mysql+lw文档+部署软件 基于Java毕业设计在线云音乐系统源码+系统+mysql+lw文档+部署软件 本源码技术栈: 项目架构:B/S架构 开发语 ...
- 云客Drupal源码分析之Session进阶
在本系列之前写过<云客Drupal源码分析之Session系统>,但那部分仅仅讲到了drupal会话的基础:Symfony的Session组件 至于drupal怎么去使用这个基础就是本主题 ...
- 云客Drupal源码分析之数据库Schema及创建数据表
本主题是<云客Drupal源码分析之数据库系统及其使用>的补充,便于查询,所以独立成一个主题 讲解数据库系统如何操作Schema(创建修改数据库.数据表.字段:判断它们的存在性等等),以及 ...
- 云客Drupal源码分析之配置系统Configuration（一）
各位<云客drupal源码分析>系列的读者: 本系列一直以每周一篇的速度进行博客原创更新,希望帮助大家理解drupal8底层原理,并缩短学习时间,但自<插件系统(上)>主题开始 ...
- 云客Drupal源码分析之节点实体访问控制处理器
以下内容仅是一个预览,完整内容请见文尾: 本篇讲解节点实体的访问控制,总结了访问检查链,对"域"."授权id"进行了清晰论述(该知识点可能是中文资料第一次提及, ...
- 云his系统源码 SaaS应用 基于Angular+Nginx+Java+Spring开发
云his系统源码 SaaS应用 功能易扩 统一对外接口管理 一.系统概述: 本套云HIS系统采用主流成熟技术开发,软件结构简洁.代码规范易阅读,SaaS应用,全浏览器访问前后端分离,多服务协同,服务可 ...
- java计算机毕业设计在线云音乐系统源码+mysql数据库+系统+lw文档+部署
java计算机毕业设计在线云音乐系统源码+mysql数据库+系统+lw文档+部署 java计算机毕业设计在线云音乐系统源码+mysql数据库+系统+lw文档+部署 本源码技术栈: 项目架构:B/S架构 ...
- Java蘑菇钉云签到管理系统源码
介绍: Java蘑菇钉云签到管理系统源码基于spring-boot-task框架开发, 功能介绍:用户通过配置个人信息实现每天定时云签到 网盘下载地址: http://kekewl.net/mJK2M ...
最新文章
- myeclipse文件目录自动定位（右编辑界面点击 左边Package Explorer导航自动定位）...
- Leetcode 47. 全排列 II (每日一题 20211015)
- Flink从入门到精通100篇（十二）-如何分析和定位 Flink 作业 OOM 问题？
- C# 系统应用之获取Windows最近使用记录
- jQuery验证码发送时间秒递减（刷新存储cookie）
- C语言编程新手自学手册下载,C语言编程新手自学手册
- Angular2之路由学习笔记
- JQ简单二级导航，加子导航栏
- 创建者模式 --- 工厂模式
- matlab免疫算法求解配送中心选址问题
- 拆弹实验-phase_2
- Windows phone 7中关于Zune软件使用几个问题
- DOM操作简易年历案例
- 【Excel】定位条件快速将空值替换为指定值
- PS照片排版1寸2寸等及照片规格
- 【Java基础】· 面向对象编程（下）习题详解
- 如何从被领导到领导别人
- android Glide 去掉绿色背景（图片变绿解决方法）
- 用onetab插件管理chrome的tab页，解决.crx安装时显示invalid的问题
- EasyExcel导出添加水印