原理
根据四元素与GPS完成点云粗拼接，先将qps与四元数与点云时间戳对齐，然后通过四元素求旋转矩阵，通过gps求平移，将gps先转换到墨卡托坐标系中，然后得出平移的变量。
主要有两个方案，一个是直接拼接到第一帧所在坐标系，还有一个是直接在第一帧位置建立与地图坐标系经度，纬度方向平行的坐标系，然后将每帧拼接到此位置，方案二的旋转可直接由四元素获得。
方案1：第一帧坐标系——【旋转:四元素，平移:gps】到地图坐标系——之后每帧要想到第一帧坐标系则通过地图坐标系x第一帧坐标系到地图坐标系的逆
方案2：第一帧坐标系——【旋转:四元素】到初始坐标系——之后每帧要想到初始坐标系坐标系则通过【旋转:四元素，平移差:gps】
惯导坐标系到雷达坐标系：【雷达与惯导没有标定出旋转矩阵，所以暂时直接手测，主要是想看一下拼接效果】
惯导坐标系设为无人机机载坐标系（x为前进方向，为正北方；z轴朝下）——>沿惯导坐标系z轴平移0.2m(t_z)——绕惯导坐标系y轴逆时针旋转rQ度(r_y;注意c++cos为弧度值)——沿惯导坐标系x轴平移0.1m(t_x)——绕惯导坐标系y轴逆时针旋转180度,然后绕z轴逆时针旋转90度（r_i_l）——>到雷达坐标系
方案一
拼接到第一帧所在坐标系：
#include "ros/ros.h"
#include <math.h>
#include <iostream>
#include <cmath>
#include "std_msgs/String.h"
#include <sensor_msgs/Imu.h>
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <geometry_msgs/TransformStamped.h>
#include "tf/transform_datatypes.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#define PI 3.1415926
#define rQ 40 //雷达旋转角度
#define TX 0.14//0.14 //平移0.18
#define TY 0
#define TZ 0.24//0.30 //0.24
#define L_I_Z 0.2//贯导z轴方向杆长
#define L_I_X 0.1//贯导x轴方向杆长
//source devel/setup.bash
//rosrun sbg_driver ros_imu_gps
//rosrun sbg_driver velodyne_imugps
//rosbag play /home/xx/catkin_lidar_imu_gps/2020-11-02-14-17-08.bag
using namespace sensor_msgs;
using namespace std;
ros::Publisher cloud_pub;
//ros::Publisher quat_translation_pub;
//ros::Publisher velodyne_pub;
int i=0;//调试
int j=1 ;//控制转雷达坐标系的循环
int k=0,kk=1,k_=0;//控制5帧循环
double gx,gy,gz ;//转到初始坐标系的平移基准
Eigen::Matrix3d rotation_matrixin;//第一帧旋转
Eigen::Matrix4d T_matrixin;//第一帧4x4旋转矩阵赋值
Eigen::Matrix4d t_z;
Eigen::Matrix4d r_y;
Eigen::Matrix4d t_x;
Eigen::Matrix4d r_i_l;
Eigen::Matrix4d matrix_1 ;
Eigen::Matrix4d matrix_2 ;
Eigen::Matrix4d matrix_3 ;
Eigen::Matrix4d matrix_4 ;
Eigen::Matrix4d matrix_5 ;
Eigen::Matrix4d matrix_6 ;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr;
pcl::PointCloud<pcl::PointXYZ> cloud_new1;
pcl::PointCloud<pcl::PointXYZ> cloud_new2;
pcl::PointCloud<pcl::PointXYZ> cloud_new3;
pcl::PointCloud<pcl::PointXYZ> cloud_new4;
pcl::PointCloud<pcl::PointXYZ> cloud_new5;
pcl::PointCloud<pcl::PointXYZ> cloud_new6;
// 把经纬坐标投影到Web墨卡托 /sys
void lonLat2WebMercator(double lon,double lat,double *x,double *y)
{
*x = lon * 20037508.34 / 180;
double ly = log(tan((90+ lat)*PI/360))/(PI/180);
*y = ly *20037508.34/180;
}
void GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z)
{
double x1,x2,y1,y2;
lonLat2WebMercator(srcLon,srcLat,&x1,&y1);
lonLat2WebMercator(destLon,destLat,&x2,&y2);
//cout<<"x1="<<x1<<endl;
//cout<<"x2="<<x2<<endl;
*x=x2-x1;
*y=y2-y1;
*z=tz-gz;
}
//pcl::visualization::CloudViewer::showCloud(boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)auto_ptr
回调函数 sys
pcl::visualization::CloudViewer viewer("viewer");
void chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pc2,const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{
i++;
double RQ=rQ*PI/180;
pcl::PointCloud<pcl::PointXYZ> cloud_tmp;//点云对象
pcl::fromROSMsg (*pc2, cloud_tmp);//cloud is the output
sensor_msgs::PointCloud2 cloud_publish;//发布拼接后的点云话题消息
点云直通滤波 ///
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_tmp (new pcl::PointCloud<pcl::PointXYZ>);//点云指针对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);//点云指针对象
cloud_filtered_tmp=cloud_tmp.makeShared();
pcl::PassThrough<pcl::PointXYZ> pass; //设置滤波器对象
pass.setInputCloud(cloud_filtered_tmp);//设置输入点云
pass.setFilterFieldName("y"); //设置过滤时所需要的点云类型的y字段
pass.setFilterLimits(-25,25); //设置在过滤字段上的范围
pass.setFilterLimitsNegative(false); //设置保留范围内的还是过滤掉范围内的
pass.filter(*cloud_filtered); //执行滤波
cloud = *cloud_filtered;
/
if(k==0)
{
//cout<<"第一次"<<endl;
//gx = msg->angular_velocity.y;//把gps值放到这里
//gy = msg->angular_velocity.x;
//gz = msg->angular_velocity.z;
double x,y,z,w;
gx = gpsmsg->position.y;//把gps值放到这里
gy = gpsmsg->position.x;//纬度
gz = gpsmsg->position.z;
x = quatmsg->quaternion.x;//四元数T_matrix(0,3)
y = quatmsg->quaternion.y;
z = quatmsg->quaternion.z;
w = quatmsg->quaternion.w;
Eigen::Quaterniond quaternion0(w,x,y,z);
Eigen::Matrix3d rotation_matrix0;
rotation_matrix0=quaternion0.toRotationMatrix();//quaternion.matrix();
rotation_matrixin =rotation_matrix0.inverse();//求逆
T_matrixin(0,0)=rotation_matrixin(0,0);
T_matrixin(1,0)=rotation_matrixin(1,0);
T_matrixin(2,0)=rotation_matrixin(2,0);
T_matrixin(3,0)=0;
T_matrixin(0,1)=rotation_matrixin(0,1);
T_matrixin(1,1)=rotation_matrixin(1,1);
T_matrixin(2,1)=rotation_matrixin(2,1);
T_matrixin(3,1)=0;
T_matrixin(0,2)=rotation_matrixin(0,2);
T_matrixin(1,2)=rotation_matrixin(1,2);
T_matrixin(2,2)=rotation_matrixin(2,2);
T_matrixin(3,2)=0;
T_matrixin(0,3)=0;
T_matrixin(1,3)=0;
T_matrixin(2,3)=0;
T_matrixin(3,3)=1;
cout<<"rotation_matrixin="<<rotation_matrixin<<endl;
//--------------------------------------------
//t_z
t_z(0,0)=1;
t_z(1,0)=0;
t_z(2,0)=0;
t_z(3,0)=0;
t_z(0,1)=0;
t_z(1,1)=1;
t_z(2,1)=0;
t_z(3,1)=0;
t_z(0,2)=0;
t_z(1,2)=0;
t_z(2,2)=1;
t_z(3,2)=0;
t_z(0,3)=TX;
t_z(1,3)=0;
t_z(2,3)=L_I_Z;//
t_z(3,3)=1;
//cout<<"t_z="<<t_z<<endl;
//r_y
cout<<"RQ="<<RQ<<endl;
r_y(0,0)=cos(RQ);
r_y(1,0)=0;
r_y(2,0)=-sin(RQ);
r_y(3,0)=0;
r_y(0,1)=0;
r_y(1,1)=1;
r_y(2,1)=0;
r_y(3,1)=0;
r_y(0,2)=sin(RQ);
r_y(1,2)=0;
r_y(2,2)=cos(RQ);
r_y(3,2)=0;
r_y(0,3)=0;
r_y(1,3)=0;
r_y(2,3)=0;//
r_y(3,3)=1;
cout<<"r_y="<<r_y<<endl;
//t_x
t_x(0,0)=1;
t_x(1,0)=0;
t_x(2,0)=0;
t_x(3,0)=0;
t_x(0,1)=0;
t_x(1,1)=1;
t_x(2,1)=0;
t_x(3,1)=0;
t_x(0,2)=0;
t_x(1,2)=0;
t_x(2,2)=1;
t_x(3,2)=0;
t_x(0,3)=L_I_X;
t_x(1,3)=0;
t_x(2,3)=0;//
t_x(3,3)=1;
//cout<<"t_x="<<t_x<<endl;
//r_i_l
r_i_l(0,0)=0;
r_i_l(1,0)=0;
r_i_l(2,0)=1;
r_i_l(3,0)=0;
r_i_l(0,1)=0;
r_i_l(1,1)=-1;
r_i_l(2,1)=0;
r_i_l(3,1)=0;
r_i_l(0,2)=1;
r_i_l(1,2)=0;
r_i_l(2,2)=0;
r_i_l(3,2)=0;
r_i_l(0,3)=0;
r_i_l(1,3)=0;
r_i_l(2,3)=0;//
r_i_l(3,3)=1;
//cout<<"r_i_l="<<r_i_l<<endl;
k++;
}
if (k!=0)
{
cout<<"i="<<i<<endl;
/// 提取出四元数与gps ///
double x,y,z,w,tx,ty,tz;
x = quatmsg->quaternion.x;//四元数
y = quatmsg->quaternion.y;
z = quatmsg->quaternion.z;
w = quatmsg->quaternion.w;
tx = gpsmsg->position.y;//把gps值放到这里
ty = gpsmsg->position.x;
tz = gpsmsg->position.z;
//cout<<" x="<< x<<endl;
/ 平移计算 /
double nx,ny,nz;
GetDirectDistance(gx,gy,gz,tx,ty,tz,&nx,&ny,&nz);//计算GPS变化量
//T_in
Eigen::Matrix4d T_in;//平移4x4旋转矩阵赋值
T_in(0,0)=1;
T_in(1,0)=0;
T_in(2,0)=0;
T_in(3,0)=0;
T_in(0,1)=0;
T_in(1,1)=1;
T_in(2,1)=0;
T_in(3,1)=0;
T_in(0,2)=0;
T_in(1,2)=0;
T_in(2,2)=1;
T_in(3,2)=0;
T_in(0,3)=ny;//y
T_in(1,3)=nx;
T_in(2,3)=-nz;
T_in(3,3)=1;
四元数转换为旋转矩阵 ///
Eigen::Quaterniond quaternion(w,x,y,z);
/*
Eigen::Quaterniond quaternion;
quaternion.x() = x;
quaternion.y() = y;
quaternion.z() = z;
quaternion.w() = w;
*/
Eigen::Matrix3d rotation_matrix;
//rotation_matrix=quaternion.toRotationMatrix();//quaternion.matrix();
rotation_matrix=quaternion.toRotationMatrix();
//cout<<"rotation_matrix="<<rotation_matrix<<endl;
Eigen::Matrix4d T_matrixi;//4x4旋转矩阵赋值
T_matrixi(0,0)=rotation_matrix(0,0);
T_matrixi(1,0)=rotation_matrix(1,0);
T_matrixi(2,0)=rotation_matrix(2,0);
T_matrixi(3,0)=0;
T_matrixi(0,1)=rotation_matrix(0,1);
T_matrixi(1,1)=rotation_matrix(1,1);
T_matrixi(2,1)=rotation_matrix(2,1);
T_matrixi(3,1)=0;
T_matrixi(0,2)=rotation_matrix(0,2);
T_matrixi(1,2)=rotation_matrix(1,2);
T_matrixi(2,2)=rotation_matrix(2,2);
T_matrixi(3,2)=0;
T_matrixi(0,3)=0;
T_matrixi(1,3)=0;
T_matrixi(2,3)=0;
T_matrixi(3,3)=1;
//最终旋转矩阵---------------------------------------------------------------
Eigen::Matrix4d T_matrix=T_matrixin*T_in*T_matrixi;//4x4旋转矩阵赋值
cout<<"旋转矩阵:T_matrix="<<T_matrix<<endl;
/*
rotation_matrix= -0.132107 0.991235 -7.72432e-05
-0.990425 -0.131996 0.0404286
0.0400641 0.00541743 0.999182
rotation_matrix= 0.955879 0.29309 0.0198578
-0.293673 0.955069 0.0399872
-0.00724572 -0.0440546 0.999003
T_matrix= 0.955879 0.29309 0.0198578 15.7504
-0.293673 0.955069 0.0399872 -4.96262
-0.00724572 -0.0440546 0.999003 0.794558
0 0 0 1
*/
四元数转换为欧拉角 ///
/*
Eigen::Quaterniond quaternion(w,x,y,z);
//Eigen::Vector3d eulerAngle(yaw,pitch,roll);
//绕z轴yaw偏航 绕y轴pitch俯仰(不能超过90度) 绕x轴roll:横滚
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);//初始化欧拉角(Z-Y-X )
欧拉角算旋转矩阵
Eigen::Matrix3d rotation_matrix;
//rotation_matrix=yawAngle*pitchAngle*rollAngle;
rotation_matrix = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
//cout<<"rotation_matrix="<<rotation_matrix<<endl;
*/
/*
rotation_matrix= -0.132107 0.991235 -7.7243e-05
-0.990425 -0.131996 0.0404286
0.0400641 0.00541743 0.999182 //zyx
*/
/ 求旋转矩阵的逆
if(j==1)
{ matrix_1 =T_matrix.inverse();}//求逆
if(j==2)
{ matrix_2 =T_matrix.inverse();}//求逆
if(j==3)
{ matrix_3 =T_matrix.inverse();}
if(j==4)
{ matrix_4 =T_matrix.inverse();}
if(j==5)
{ matrix_5 =T_matrix.inverse();}
if(j==6)
{
matrix_6 =T_matrix.inverse();//求逆
j=0;
}
j++;
//cout<<"matrix_1="<<matrix_1<<endl;
// 点云处理
float x_pass;
float y_pass;
float z_pass;
for (int i = 0; i <cloud.size(); i++)
{
// 按飞机——雷达坐标赋值//点云变成机体坐标系中的点
if(cloud.points[i].x<2.5&&cloud.points[i].y<2.5&&cloud.points[i].z<2.5)
{
cloud.points[i].x= 0 ;
cloud.points[i].y= 0 ;
cloud.points[i].z= 0 ;
}
//-------------------------------------------------------------------------------------------------------------------------------------
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud.points[i].x;
pointxyz(1,0)=cloud.points[i].y;
pointxyz(2,0)=cloud.points[i].z;
pointxyz(3,0)=1;
Eigen::MatrixXd m=t_z*r_y*t_x*r_i_l*pointxyz;
//x_pass=lidartoimu(0,0)+TX;
//y_pass=lidartoimu(1,0)+TY;
//z_pass=lidartoimu(2,0)+TZ;
//===========================
//Eigen::MatrixXd pointxyz2(3,1);//点云坐标
//pointxyz2(0,0)=x_pass;
//pointxyz2(1,0)=y_pass;
//pointxyz2(2,0)=z_pass;
//Eigen::MatrixXd lidartoimu2=re_y*pointxyz2;
//---------------------------------------------------------------------------------------------------------------------------------------
///
//把点云坐标转成矩阵
//Eigen::MatrixXd m(4,1);
//m(0,0)=x_pass;
//m(1,0)=y_pass;
//m(2,0)=z_pass;
//m(3,0)=1;
//====================
//m(0,0)=lidartoimu2(0,0);
//m(1,0)=lidartoimu2(1,0);
//m(2,0)=lidartoimu2(2,0);
//m(3,0)=1;
//--------------------------------
//cout<<" x_pass= "<< x_pass<<endl;
//Eigen::MatrixXd n=matrix_33*m;// 矩阵与旋转矩阵相乘
//Eigen::MatrixXd n=rotation_matrix*m;
Eigen::MatrixXd n=T_matrix*m;
/
/
// cout<<" n= "<< n<<endl;
/// 把计算后的坐标赋值给点云
// 按惯导——雷达坐标赋值
/*
x_pass= n(0,0) +ny;
y_pass= n(1,0) -nz;
z_pass= n(2,0) +nx;
*/
// 按飞机——雷达坐标赋值
// x_pass= n(0,0) ;//+ny;
//y_pass= n(1,0) ;//+nx;
// z_pass= n(2,0) ;//-nz;
// pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/ 5帧迭代写入(全转回初始坐标系) /
/*
if(k==6||k==7||((k>=2&&k<=4)&&(k_==1)))
{
cloud_new6.push_back(point_1);
}if((k>=5&&k<=7)||((2<=k<=3)&&(k_==1)))
{
cloud_new5.push_back(point_1);
}if((k>=4&&k<=7)||(k==2&&k_==1))
{
cloud_new4.push_back(point_1);
}
if(k>=3&&k<=7)
{
cloud_new3.push_back(point_1);
}if(k>=2&&k<=6)
{
cloud_new2.push_back(point_1);
}if( k>=1&&k<=5||k==7)
{
cloud_new1.push_back(point_1);
}
*/
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/ 5帧迭代写入(转回雷达坐标系) /p
//x=n_6(0,0) &惯导坐标系 x=n_6(2,0)-0.3 &雷达坐标系
//y=n_6(1,0) y=0.14-n_6(1,0)
//z=n_6(2,0) z=n_6(0,0)
/*
if(k==6||k==7||((k>=2&&k<=4)&&(k_==1)))
{
Eigen::MatrixXd n_6=matrix_6*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_6( (n_6(2,0)-0.3), (0.14-n_6(1,0)), n_6(0,0) );
cloud_new6.push_back(point_6);
}if((k>=5&&k<=7)||((k==2||k==3)&&(k_==1)))
{
Eigen::MatrixXd n_5=matrix_5*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_5((n_5(2,0)-0.3), (0.14-n_5(1,0)), n_5(0,0) );
cloud_new5.push_back(point_5);
}if((k>=4&&k<=7)||(k==2&&k_==1))
{
Eigen::MatrixXd n_4=matrix_4*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_4((n_4(2,0)-0.3), (0.14-n_4(1,0)), n_4(0,0) );
cloud_new4.push_back(point_4);
}
if(k>=3&&k<=7)
{
Eigen::MatrixXd n_3=matrix_3*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_3((n_3(2,0)-0.3), (0.14-n_3(1,0)), n_3(0,0) );
cloud_new3.push_back(point_3);
}if(k>=2&&k<=6)
{
Eigen::MatrixXd n_2=matrix_2*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_2((n_2(2,0)-0.3), (0.14-n_2(1,0)), n_2(0,0) );
cloud_new2.push_back(point_2);
}if( k>=1&&k<=5||k==7)
{
Eigen::MatrixXd n_1=matrix_1*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_1((n_1(2,0)-0.3), (0.14-n_1(1,0)), n_1(0,0) );
cloud_new1.push_back(point_1);
}
*/
// 调试-转换到第一个坐标系 /
//Eigen::MatrixXd n_1=matrix_1*n;// 与旋转矩阵的逆相乘
//Eigen::MatrixXd n=T_matrix*m;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new1.push_back(point_1);
/*
调试3帧拼接 /
if( k==40||k==70 ||k==100||k==130) //k==65
{
cloud_new1.push_back(point_1);
}
*/
}//for
// 5帧迭代 /
/*
if(kk==10)
{
cloud_ptr=cloud_new6.makeShared();
pcl::toROSMsg(cloud_new6, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new5.clear();
kk=4;
}if(kk==9)
{
cloud_ptr=cloud_new5.makeShared();
pcl::toROSMsg(cloud_new5, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new4.clear();
}if(kk==8)
{
cloud_ptr=cloud_new4.makeShared();//5
pcl::toROSMsg(cloud_new4, cloud_publish);
viewer.showCloud(cloud_ptr);
cloud_new3.clear();
}
if(kk==7)
{
cloud_ptr=cloud_new3.makeShared();//5
pcl::toROSMsg(cloud_new3, cloud_publish);
viewer.showCloud(cloud_ptr);
cloud_new2.clear();
}if(kk==6 )
{
cloud_ptr=cloud_new2.makeShared();
pcl::toROSMsg(cloud_new2, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new1.clear();
}if((kk>=1&&kk<=3)||kk==5||(kk==4&&k_==0))
{
cloud_ptr=cloud_new1.makeShared();
pcl::toROSMsg(cloud_new1, cloud_publish);
viewer.showCloud(cloud_ptr);
if(kk==5)
{ cloud_new6.clear(); }
}
// 控制循环 /
if(k==7)
{ k=1;
k_=1;
}
*/
cloud_ptr=cloud_new1.makeShared();
pcl::toROSMsg(cloud_new1, cloud_publish);
viewer.showCloud(cloud_ptr);
kk++;
k++;
/// 发布点云消息 //
cloud_publish.header.stamp = quatmsg->header.stamp;
cloud_publish.header.frame_id = "map";
cloud_pub.publish(cloud_publish);
// 调试 /sys
//cloud_ptr=cloud_new1.makeShared();
//viewer.showCloud(cloud_ptr);
//pcl::io::savePCDFileASCII("/home/xx/200.pcd",*cloud_ptr);
} //else
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ros_velodyne");
ros::NodeHandle n;
cloud_pub = n.advertise<sensor_msgs::PointCloud2>("cloud_new", 1000);//拼接后的点云发布
//quat_translation_pub = n.advertise<sensor_msgs::Imu>("quat_translation", 1000);
message_filters::Subscriber<sensor_msgs::PointCloud2> lidar(n,"/velodyne_points",1000);
message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 1000);
message_filters::Subscriber<sbg_driver::SbgEkfQuat> quat_sub(n, "/sbg/ekf_quat", 1000); // topic1 输入
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sbg_driver::SbgEkfNav,sbg_driver::SbgEkfQuat> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000),lidar, gps_sub,quat_sub); //queue size=10 // 同步
sync.registerCallback(boost::bind(&chatterCallback, _1, _2,_3)); // 回调
//cout<<"velodyne_points open success"<<endl;
//ros::Rate loop(5);
ros::spin();
return 0;
}
方案二
以第一帧GPS位置建立一个坐标系，x轴平行于地图坐标系经度方向，y轴平行于地图坐标系纬度方向，然后将以后的点云拼接到此坐标系下。
#include "ros/ros.h"
#include <math.h>
#include <iostream>
#include <cmath>
#include "std_msgs/String.h"
#include <sensor_msgs/Imu.h>
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <geometry_msgs/TransformStamped.h>
#include "tf/transform_datatypes.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#define PI 3.1415926
#define rQ 40 //雷达旋转角度
//#define rQ_1 40 //雷达旋转角度
#define TX 0.18//0.14 //平移0.18
#define TY 40
#define TZ 0.24//0.30 //0.24
//source devel/setup.bash
//rosrun sbg_driver ros_imu_gps
//rosrun sbg_driver velodyne_imugps
//rosbag play /home/xx/catkin_lidar_imu_gps/2020-11-02-14-17-08.bag
using namespace sensor_msgs;
using namespace std;
ros::Publisher cloud_pub;
//ros::Publisher quat_translation_pub;
//ros::Publisher velodyne_pub;
int i=0;//调试
int j=1 ;//控制转雷达坐标系的循环
int k=0,kk=1,k_=0;//控制5帧循环
double gx,gy,gz ;//转到初始坐标系的平移基准
Eigen::Matrix3d rotation_matrixin;
Eigen::Matrix4d matrix_1 ;
Eigen::Matrix4d matrix_2 ;
Eigen::Matrix4d matrix_3 ;
Eigen::Matrix4d matrix_4 ;
Eigen::Matrix4d matrix_5 ;
Eigen::Matrix4d matrix_6 ;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr;
pcl::PointCloud<pcl::PointXYZ> cloud_new1;
pcl::PointCloud<pcl::PointXYZ> cloud_new2;
pcl::PointCloud<pcl::PointXYZ> cloud_new3;
pcl::PointCloud<pcl::PointXYZ> cloud_new4;
pcl::PointCloud<pcl::PointXYZ> cloud_new5;
pcl::PointCloud<pcl::PointXYZ> cloud_new6;
// 把经纬坐标投影到Web墨卡托 /sys
void lonLat2WebMercator(double lon,double lat,double *x,double *y)
{
*x = lon * 20037508.34 / 180;
double ly = log(tan((90+ lat)*PI/360))/(PI/180);
*y = ly *20037508.34/180;
}
void GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z)
{
double x1,x2,y1,y2;
lonLat2WebMercator(srcLon,srcLat,&x1,&y1);
lonLat2WebMercator(destLon,destLat,&x2,&y2);
//cout<<"x1="<<x1<<endl;
//cout<<"x2="<<x2<<endl;
*x=x2-x1;
*y=y2-y1;
*z=tz-gz;
}
//pcl::visualization::CloudViewer::showCloud(boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> const> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)auto_ptr
回调函数 sys
pcl::visualization::CloudViewer viewer("viewer");
void chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pc2,const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{
i++;
double RQ=rQ*PI/180;
pcl::PointCloud<pcl::PointXYZ> cloud_tmp;//点云对象
pcl::fromROSMsg (*pc2, cloud_tmp);//cloud is the output
sensor_msgs::PointCloud2 cloud_publish;//发布拼接后的点云话题消息
点云直通滤波 ///
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_tmp (new pcl::PointCloud<pcl::PointXYZ>);//点云指针对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);//点云指针对象
cloud_filtered_tmp=cloud_tmp.makeShared();
pcl::PassThrough<pcl::PointXYZ> pass; //设置滤波器对象
pass.setInputCloud(cloud_filtered_tmp);//设置输入点云
pass.setFilterFieldName("y"); //设置过滤时所需要的点云类型的y字段
pass.setFilterLimits(-66,66); //设置在过滤字段上的范围
pass.setFilterLimitsNegative(false); //设置保留范围内的还是过滤掉范围内的
pass.filter(*cloud_filtered); //执行滤波
cloud = *cloud_filtered;
/
if(k==0)
{
//cout<<"第一次"<<endl;
//gx = msg->angular_velocity.y;//把gps值放到这里
//gy = msg->angular_velocity.x;
//gz = msg->angular_velocity.z;
double x,y,z,w;
gx = gpsmsg->position.y;//把gps值放到这里
gy = gpsmsg->position.x;
gz = gpsmsg->position.z;
x = quatmsg->quaternion.x;//四元数
y = quatmsg->quaternion.y;
z = quatmsg->quaternion.z;
w = quatmsg->quaternion.w;
Eigen::Quaterniond quaternion0(w,x,y,z);
Eigen::Matrix3d rotation_matrix0;
//rotation_matrix0=quaternion0.toRotationMatrix();//quaternion.matrix();
//rotation_matrixin =rotation_matrix0.inverse();//求逆
k++;
}
if (k!=0)
{
cout<<"iiiiiiii="<<i<<endl;
/// 提取出四元数与gps ///
double x,y,z,w,tx,ty,tz;
x = quatmsg->quaternion.x;//四元数
y = quatmsg->quaternion.y;
z = quatmsg->quaternion.z;
w = quatmsg->quaternion.w;
tx = gpsmsg->position.y;//把gps值放到这里
ty = gpsmsg->position.x;
tz = gpsmsg->position.z;
//cout<<" x="<< x<<endl;
/ 平移计算 /
double nx,ny,nz;
GetDirectDistance(gx,gy,gz,tx,ty,tz,&nx,&ny,&nz);//计算GPS变化量
//cout<<"nx= "<<nx<<endl;
//cout<<"ny= "<<ny<<endl;
//cout<<"nz= "<<nz<<endl;
四元数转换为旋转矩阵 ///
Eigen::Quaterniond quaternion(w,x,y,z);
/*
Eigen::Quaterniond quaternion;
quaternion.x() = x;
quaternion.y() = y;
quaternion.z() = z;
quaternion.w() = w;
*/
Eigen::Matrix3d rotation_matrix;
//rotation_matrix=quaternion.toRotationMatrix();//quaternion.matrix();
//rotation_matrix=rotation_matrixin*quaternion.toRotationMatrix();
rotation_matrix=quaternion.toRotationMatrix();
//cout<<"rotation_matrix="<<rotation_matrix<<endl;
Eigen::Matrix4d T_matrix;//4x4旋转矩阵赋值
T_matrix(0,0)=rotation_matrix(0,0);
T_matrix(1,0)=rotation_matrix(1,0);
T_matrix(2,0)=rotation_matrix(2,0);
T_matrix(3,0)=0;
T_matrix(0,1)=rotation_matrix(0,1);
T_matrix(1,1)=rotation_matrix(1,1);
T_matrix(2,1)=rotation_matrix(2,1);
T_matrix(3,1)=0;
T_matrix(0,2)=rotation_matrix(0,2);
T_matrix(1,2)=rotation_matrix(1,2);
T_matrix(2,2)=rotation_matrix(2,2);
T_matrix(3,2)=0;
T_matrix(0,3)=ny;//ny
T_matrix(1,3)=nx;
T_matrix(2,3)=-nz;
T_matrix(3,3)=1;
//cout<<"T_matrix="<<T_matrix<<endl;
/*
rotation_matrix= -0.132107 0.991235 -7.72432e-05
-0.990425 -0.131996 0.0404286
0.0400641 0.00541743 0.999182
rotation_matrix= 0.955879 0.29309 0.0198578
-0.293673 0.955069 0.0399872
-0.00724572 -0.0440546 0.999003
T_matrix= 0.955879 0.29309 0.0198578 15.7504
-0.293673 0.955069 0.0399872 -4.96262
-0.00724572 -0.0440546 0.999003 0.794558
0 0 0 1
*/
四元数转换为欧拉角 ///
/*
Eigen::Quaterniond quaternion(w,x,y,z);
//Eigen::Vector3d eulerAngle(yaw,pitch,roll);
//绕z轴yaw偏航 绕y轴pitch俯仰(不能超过90度) 绕x轴roll:横滚
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);//初始化欧拉角(Z-Y-X )
欧拉角算旋转矩阵
Eigen::Matrix3d rotation_matrix;
//rotation_matrix=yawAngle*pitchAngle*rollAngle;
rotation_matrix = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
//cout<<"rotation_matrix="<<rotation_matrix<<endl;
*/
/*
rotation_matrix= -0.132107 0.991235 -7.7243e-05
-0.990425 -0.131996 0.0404286
0.0400641 0.00541743 0.999182 //zyx
*/
/ 求旋转矩阵的逆
if(j==1)
{ matrix_1 =T_matrix.inverse();}//求逆
if(j==2)
{ matrix_2 =T_matrix.inverse();}//求逆
if(j==3)
{ matrix_3 =T_matrix.inverse();}
if(j==4)
{ matrix_4 =T_matrix.inverse();}
if(j==5)
{ matrix_5 =T_matrix.inverse();}
if(j==6)
{
matrix_6 =T_matrix.inverse();//求逆
j=0;
}
j++;
//cout<<"matrix_1="<<matrix_1<<endl;
// 点云处理
float x_pass;
float y_pass;
float z_pass;
Eigen::Matrix3d r_y;//3x3矩阵绕Y轴
r_y(0,0)=cos(RQ);
r_y(0,1)=0;
r_y(0,2)=sin(RQ);
r_y(1,0)=0;
r_y(1,1)=1;
r_y(1,2)=0;
r_y(2,0)=-sin(RQ);
r_y(2,1)=0;
r_y(2,2)=cos(RQ);
Eigen::Matrix3d r_lidartoimu;//3x3矩阵雷达到贯导
r_lidartoimu(0,0)=0;
r_lidartoimu(0,1)=0;
r_lidartoimu(0,2)=1;
r_lidartoimu(1,0)=0;
r_lidartoimu(1,1)=-1;
r_lidartoimu(1,2)=0;
r_lidartoimu(2,0)=1;
r_lidartoimu(2,1)=0;
r_lidartoimu(2,2)=0;
for (int i = 0; i <cloud.size(); i++)
{
// 按飞机——雷达坐标赋值//点云变成机体坐标系中的点
//-------------------------------------------------------------------------------------------------------------------------------------
if(cloud.points[i].x<2.5&&cloud.points[i].y<2.5&&cloud.points[i].z<2.5)
{
cloud.points[i].x= 0 ;
cloud.points[i].y= 0 ;
cloud.points[i].z= 0 ;
}
Eigen::MatrixXd pointxyz(3,1);//点云坐标
pointxyz(0,0)=cloud.points[i].x;
pointxyz(1,0)=cloud.points[i].y;
pointxyz(2,0)=cloud.points[i].z;
//Eigen::MatrixXd lidartoimu=r_lidartoimu*r_y*pointxyz;
Eigen::MatrixXd lidartoimu=r_y*r_lidartoimu*pointxyz;
x_pass=lidartoimu(0,0)+TX;
y_pass=lidartoimu(1,0)+TY;
z_pass=lidartoimu(2,0)+TZ;
//===========================
//Eigen::MatrixXd pointxyz2(3,1);//点云坐标
//pointxyz2(0,0)=x_pass;
//pointxyz2(1,0)=y_pass;
//pointxyz2(2,0)=z_pass;
//Eigen::MatrixXd lidartoimu2=re_y*pointxyz2;
//---------------------------------------------------------------------------------------------------------------------------------------
///
//把点云坐标转成矩阵
Eigen::MatrixXd m(4,1);
m(0,0)=x_pass;
m(1,0)=y_pass;
m(2,0)=z_pass;
m(3,0)=1;
//cout<<" x_pass= "<< x_pass<<endl;
//Eigen::MatrixXd n=matrix_33*m;// 矩阵与旋转矩阵相乘
//Eigen::MatrixXd n=rotation_matrix*m;
Eigen::MatrixXd n=T_matrix*m;
/
/
// cout<<" n= "<< n<<endl;
/// 把计算后的坐标赋值给点云
// 按惯导——雷达坐标赋值
/*
x_pass= n(0,0) +ny;
y_pass= n(1,0) -nz;
z_pass= n(2,0) +nx;
*/
// 按飞机——雷达坐标赋值
// x_pass= n(0,0) ;//+ny;
//y_pass= n(1,0) ;//+nx;
// z_pass= n(2,0) ;//-nz;
// pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/ 5帧迭代写入(全转回初始坐标系) /
/*
if(k==6||k==7||((k>=2&&k<=4)&&(k_==1)))
{
cloud_new6.push_back(point_1);
}if((k>=5&&k<=7)||((2<=k<=3)&&(k_==1)))
{
cloud_new5.push_back(point_1);
}if((k>=4&&k<=7)||(k==2&&k_==1))
{
cloud_new4.push_back(point_1);
}
if(k>=3&&k<=7)
{
cloud_new3.push_back(point_1);
}if(k>=2&&k<=6)
{
cloud_new2.push_back(point_1);
}if( k>=1&&k<=5||k==7)
{
cloud_new1.push_back(point_1);
}
*/
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/ 5帧迭代写入(转回雷达坐标系) /p
//x=n_6(0,0) &惯导坐标系 x=n_6(2,0)-0.3 &雷达坐标系
//y=n_6(1,0) y=0.14-n_6(1,0)
//z=n_6(2,0) z=n_6(0,0)
/*
if(k==6||k==7||((k>=2&&k<=4)&&(k_==1)))
{
Eigen::MatrixXd n_6=matrix_6*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_6( (n_6(2,0)-0.3), (0.14-n_6(1,0)), n_6(0,0) );
cloud_new6.push_back(point_6);
}if((k>=5&&k<=7)||((k==2||k==3)&&(k_==1)))
{
Eigen::MatrixXd n_5=matrix_5*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_5((n_5(2,0)-0.3), (0.14-n_5(1,0)), n_5(0,0) );
cloud_new5.push_back(point_5);
}if((k>=4&&k<=7)||(k==2&&k_==1))
{
Eigen::MatrixXd n_4=matrix_4*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_4((n_4(2,0)-0.3), (0.14-n_4(1,0)), n_4(0,0) );
cloud_new4.push_back(point_4);
}
if(k>=3&&k<=7)
{
Eigen::MatrixXd n_3=matrix_3*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_3((n_3(2,0)-0.3), (0.14-n_3(1,0)), n_3(0,0) );
cloud_new3.push_back(point_3);
}if(k>=2&&k<=6)
{
Eigen::MatrixXd n_2=matrix_2*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_2((n_2(2,0)-0.3), (0.14-n_2(1,0)), n_2(0,0) );
cloud_new2.push_back(point_2);
}if( k>=1&&k<=5||k==7)
{
Eigen::MatrixXd n_1=matrix_1*n;// 与旋转矩阵的逆相乘
pcl::PointXYZ point_1((n_1(2,0)-0.3), (0.14-n_1(1,0)), n_1(0,0) );
cloud_new1.push_back(point_1);
}
*/
// 调试-转换到第一个坐标系 /
//Eigen::MatrixXd n_1=matrix_1*n;// 与旋转矩阵的逆相乘
//Eigen::MatrixXd n=T_matrix*m;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new1.push_back(point_1);
/*
调试3帧拼接 /
if( k==40||k==70 ||k==100||k==130) //k==65
{
cloud_new1.push_back(point_1);
}
*/
}//for
// 5帧迭代 /
/*
if(kk==10)
{
cloud_ptr=cloud_new6.makeShared();
pcl::toROSMsg(cloud_new6, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new5.clear();
kk=4;
}if(kk==9)
{
cloud_ptr=cloud_new5.makeShared();
pcl::toROSMsg(cloud_new5, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new4.clear();
}if(kk==8)
{
cloud_ptr=cloud_new4.makeShared();//5
pcl::toROSMsg(cloud_new4, cloud_publish);
viewer.showCloud(cloud_ptr);
cloud_new3.clear();
}
if(kk==7)
{
cloud_ptr=cloud_new3.makeShared();//5
pcl::toROSMsg(cloud_new3, cloud_publish);
viewer.showCloud(cloud_ptr);
cloud_new2.clear();
}if(kk==6 )
{
cloud_ptr=cloud_new2.makeShared();
pcl::toROSMsg(cloud_new2, cloud_publish);
viewer.showCloud(cloud_ptr);//5
cloud_new1.clear();
}if((kk>=1&&kk<=3)||kk==5||(kk==4&&k_==0))
{
cloud_ptr=cloud_new1.makeShared();
pcl::toROSMsg(cloud_new1, cloud_publish);
viewer.showCloud(cloud_ptr);
if(kk==5)
{ cloud_new6.clear(); }
}
// 控制循环 /
if(k==7)
{ k=1;
k_=1;
}
*/
cloud_ptr=cloud_new1.makeShared();
pcl::toROSMsg(cloud_new1, cloud_publish);
viewer.showCloud(cloud_ptr);
kk++;
k++;
/// 发布点云消息 //
cloud_publish.header.stamp = quatmsg->header.stamp;
cloud_publish.header.frame_id = "map";
cloud_pub.publish(cloud_publish);
// 调试 /sys
//cloud_ptr=cloud_new1.makeShared();
//viewer.showCloud(cloud_ptr);
//pcl::io::savePCDFileASCII("/home/xx/200.pcd",*cloud_ptr);
} //else
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ros_velodyne");
ros::NodeHandle n;
cloud_pub = n.advertise<sensor_msgs::PointCloud2>("cloud_new", 1000);//拼接后的点云发布
//quat_translation_pub = n.advertise<sensor_msgs::Imu>("quat_translation", 1000);
message_filters::Subscriber<sensor_msgs::PointCloud2> lidar(n,"/velodyne_points",1000);
message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 1000);
message_filters::Subscriber<sbg_driver::SbgEkfQuat> quat_sub(n, "/sbg/ekf_quat", 1000); // topic1 输入
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sbg_driver::SbgEkfNav,sbg_driver::SbgEkfQuat> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(1000),lidar, gps_sub,quat_sub); //queue size=10 // 同步
sync.registerCallback(boost::bind(&chatterCallback, _1, _2,_3)); // 回调
//cout<<"velodyne_points open success"<<endl;
//ros::Rate loop(5);
ros::spin();
return 0;
}
结果
注意：经度方向为坐标系x轴，纬度方向为y轴
2681
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
