激光雷达与组合贯导（imu）联合标定方案三（含源码）_激光雷达和惯导标定

原理

拿纸箱或角反射器作标记物，记录标记点gps位置，让无人机携带激光雷达与组合贯导扫描标记点，提取出含标记点的某一帧点云及其对应的四元素与gps，然后可以通过matlab提取出标记物点云坐标，同时可以根据四元素与gps计算出此时标记点在贯导坐标系中的坐标。按照这种方法，改变标记物位置，获取多组点对，然后通过ICP算法求解出雷达到贯导之间的旋转矩阵。

提取单帧点云及其对应的四元素与gps，并求解出贯导坐标系下的标记点坐标

为了方便提取角点，保存单帧点云前先对其进行滤波处理
为了获得该帧对应的四元素与gps，对点云与贯导获取的数据先时间戳对齐

求解贯导坐标系下的标记点坐标

获取的gps并不能直接用，首先先将gps投影到墨卡托坐标系下，计算出其在地图坐标系(M)下贯导与标记点§间的平移关系：

这里的平移并不是贯导坐标系下的标记点坐标，假设贯导坐标系原点处还有一个与地图坐标系经纬轴平行的坐标系B，关系如图：

【这里为了方便理解，贯导坐标系M与B坐标系原点没有重合】
通过坐标系转换可知B坐标系下的角点坐标为：

通过四元数获得贯导坐标系M到B坐标系的旋转R，B坐标系下的角点坐标左乘旋转矩阵R的逆即可得到惯导坐标系下的角点坐标：

代码

//*****************************************************************************************************
//     保存单帧点云中的角反射器角点及其在imu坐标系下的坐标
//     
//    source devel/setup.bash && rosrun cam_laser_calib pcd_points_gps 5 111.pcd   //5表示保存第5帧，111.pcd表示保存的pcd文件名
//  （我直接在相机雷达标定的功能包里写的，所以包名为“cam_laser_calib” ）
//    pcl_viewer 111.pcd   
//**************************************************************************************************


#include<ros/ros.h>
#include<iostream>
#include<fstream>
#include<sstream>
#include<string>
#include<vector>
#include <math.h>

#include <stdio.h>
#include<sensor_msgs/PointCloud2.h>  
#include "std_msgs/String.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/filter.h>
#include "tf/transform_datatypes.h"
#include <tf2_msgs/TFMessage.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/passthrough.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 <pcl_ros/point_cloud.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/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>


//角反射器的经纬坐标
double gj=126.62823496;
double gw=45.71438903;
double gz=138.263;


int i=1;
int m=0;//保存第m帧

std::string pcd_mame;

#define PI 3.1415926
using namespace std;
using namespace pcl;

//把经纬坐标投影到Web墨卡托 

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);
    *x=x2-x1;
    *y=y2-y1;
    *z=tz-gz;

}


pcl::visualization::CloudViewer viewer("viewer");

void chatterCallback(const  boost::shared_ptr<const sensor_msgs::PointCloud2>& pointmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const  boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{

   cout<<"i="<<i<<endl;

   pcl::PCLPointCloud2 pcl_pc2;
   pcl_conversions::toPCL(*pointmsg,pcl_pc2);

   pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromPCLPointCloud2(pcl_pc2,*tmp_cloud);

   //  点云直通滤波 
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr; 
    pcl::PointCloud<pcl::PointXYZ> cloud_new;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);//点云指针对象
   
    pcl::PassThrough<pcl::PointXYZ> pass; //设置滤波器对象
    pass.setInputCloud(tmp_cloud);//设置输入点云
    pass.setFilterFieldName("y"); //设置过滤时所需要的点云类型的y字段
    pass.setFilterLimits(-6,6); //设置在过滤字段上的范围
    pass.setFilterLimitsNegative(false); //设置保留范围内的还是过滤掉范围内的
    pass.filter(*cloud_filtered); //执行滤波

    cloud = *cloud_filtered;
   //-----------------------------------------------------------------------------------
    for (int k = 0; k <cloud.size(); k++)
    {
       if(cloud.points[k].x<2.5&&cloud.points[k].y<2.5&&cloud.points[k].z<2.5)
        {
            cloud.points[k].x= 0 ;
            cloud.points[k].y= 0 ;
            cloud.points[k].z= 0 ;
         }

     }
    cloud_ptr=cloud.makeShared();//转为指针类型
    viewer.showCloud(cloud_ptr); 


  if(i==m)

  {

   //保存单帧点云
   
    pcl::io::savePCDFileASCII(pcd_mame,*cloud_ptr);//*tmp_cloud
    printf("成功保存第%d帧，并命名为：%s\n",m,pcd_mame.c_str());

    //提取四元素与gps
     double x,y,z,w,tj,tw,tz;
    
     x = quatmsg->quaternion.x;//四元数
     y = quatmsg->quaternion.y;
     z = quatmsg->quaternion.z;
     w = quatmsg->quaternion.w;
     tj = gpsmsg->position.y;//jin
     tw = gpsmsg->position.x;//wei
     tz = gpsmsg->position.z;
     //cout<<"tz==="<<tz<<endl;
     //平移计算  

     double nj,nw,nz;
     GetDirectDistance(gj,gw,gz,tj,tw,tz,&nj,&nw,&nz);//计算GPS变化量
     Eigen::MatrixXd map_imu(3,1);//map坐标系下的imu坐标
     map_imu(0,0)=nw;
     map_imu(1,0)=nj;
     map_imu(2,0)=-nz;
    // 四元数转换为旋转矩阵 

     Eigen::Quaterniond quaternion(w,x,y,z);
     Eigen::Matrix3d rotation_matrix;
     rotation_matrix=quaternion.toRotationMatrix();
     Eigen::Matrix3d matrix_1;//求逆
     matrix_1 =rotation_matrix.inverse();
   //imu坐标系下的角点坐标 

    Eigen::MatrixXd imu_xyz=matrix_1*map_imu;
    FILE *fpWrite=fopen("imu_xyz.txt","a");
    fprintf(fpWrite,"%16.10f %16.10f %16.10f\n",imu_xyz(0,0),imu_xyz(1,0),imu_xyz(2,0));
    fclose(fpWrite);

  }


  i++;

}



int main(int argc,char** argv)
{
  ros::init (argc, argv, "ros_points");
  ros::NodeHandle n;

  string m_=argv[1];
  m= atoi(m_.c_str());
  printf("保存第%d帧\n",m);
  pcd_mame = argv[2]; 
  cout<<"保存的pcd点云名为："<<pcd_mame<<'\n'; 

  message_filters::Subscriber<sensor_msgs::PointCloud2> points_sub(n, "/velodyne_points", 2000);
  message_filters::Subscriber<sbg_driver::SbgEkfNav> gps_sub(n, "/sbg/ekf_nav", 2000);     
  message_filters::Subscriber<sbg_driver::SbgEkfQuat> quat_sub(n, "/sbg/ekf_quat", 2000);   
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sbg_driver::SbgEkfNav,sbg_driver::SbgEkfQuat> MySyncPolicy;//sensor_msgs::PointCloud2,
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(2000),points_sub,gps_sub, quat_sub);  // 同步points_sub,
  sync.registerCallback(boost::bind(&chatterCallback, _1,_2,_3));   // _1, 

 
  ros::spin ();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211

提取点云坐标

方法一：通过matlab提取
将保存的单帧点云用matlab打开，拾取对应点云坐标。。。

clc
Pc=pcread('.\pcd\111.pcd');
%打开点云文件
pcshow(Pc)%显示点云

1
2
3
4
5

方法二：pcl程序提取

//*****************************************************************************************************
//    获得单帧点云中的角反射器角点
//
//    按中shift键,然后在角点处鼠标左击
//    source devel/setup.bash && rosrun cam_laser_calib pcd_points_get 111.pcd   //111.pcd  表示打开的pcd点云文件名
//    pcl_viewer 111.pcd
//**************************************************************************************************



#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
 
using PointT = pcl::PointXYZRGB;
using PointCloudT = pcl::PointCloud<PointT>;
 
// 用于将参数传递给回调函数的结构体
struct CallbackArgs {
    PointCloudT::Ptr clicked_points_3d;
    pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};
 
void pickPointCallback(const pcl::visualization::PointPickingEvent &event, void *args) {
    CallbackArgs *data = (CallbackArgs *) args;
    if (event.getPointIndex() == -1)
        return;
    PointT current_point;
    event.getPoint(current_point.x, current_point.y, current_point.z);
    data->clicked_points_3d->points.push_back(current_point);
    // 绘制红色点
    pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
    data->viewerPtr->removePointCloud("clicked_points");
    data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
    data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10,
                                                      "clicked_points");
    std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;

    FILE *fpWrite=fopen("lidar_xyz.txt","a");
    fprintf(fpWrite,"%16.10f %16.10f %16.10f\n",current_point.x,current_point.y,current_point.z);
    fclose(fpWrite);
    
}
 
int main(int argc, char *argv[]) {

    std::string b = argv[1]; 
    std::cout<<"打开pcd点云文件："<<b<<'\n'; 
    std::string file_name(b);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
    // 加载点云
    if (pcl::io::loadPCDFile(file_name, *cloud) == -1) {
        std::cerr << "could not load file: " << file_name << std::endl;
    }
    std::cout << cloud->points.size() << std::endl;
    // 显示点云
    viewer->addPointCloud(cloud, "cloud");
    viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
    // 添加点拾取回调函数
    CallbackArgs  cb_args;
    PointCloudT::Ptr clicked_points_3d(new PointCloudT);
    cb_args.clicked_points_3d = clicked_points_3d;
    cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
    viewer->registerPointPickingCallback(pickPointCallback, (void*)&cb_args);
    std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
 
    viewer->spin();
    std::cout << "done." << std::endl;
 
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }
    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

matlab的ICP计算

clear;
close all;
clc;

data_source=load('imu.txt');%惯导坐标点
data_target=load('lidar.txt');%雷达坐标点
data_source=data_source';
data_target=data_target';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%绘制两幅原始图像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b');
hold on;
scatter3(x2,y2,z2,'r');
hold off;
 
 
T_final=eye(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %初次更新点集（代表粗配准结果）
err=1;
while(err>0.001)
    iteration=iteration+1;    %迭代次数
    disp(['迭代次数ieration=',num2str(iteration)]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %利用欧式距离找出对应点集
    k=size(data_target,2);
    for i = 1:k
        data_q1(1,:) = data_source(1,:) - data_target(1,i);    % 两个点集中的点x坐标之差
        data_q1(2,:) = data_source(2,:) - data_target(2,i);    % 两个点集中的点y坐标之差
        data_q1(3,:) = data_source(3,:) - data_target(3,i);    % 两个点集中的点z坐标之差
        distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2;  % 欧氏距离
        [min_dis, min_index] = min(distance);   % 找到距离最小的那个点
        data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点
        error(i) = min_dis;     % 保存距离差值
    end
     
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %去中心化
    data_target_mean=mean(data_target,2);
    data_mid_mean=mean(data_mid,2);
    data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));
    data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));
     
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %SVD分解
    W=zeros(3,3);
    for j=1:size(data_target_c,2)
        W=W+data_mid_c(:,j)*data_target_c(:,j)';
    end
    [U,S,V]=svd(W);
    Rf=U*V';
    Tf=data_mid_mean-Rf*data_target_mean;
    err=mean(error);
    T_t=[Rf,Tf];
    T_t=[T_t;0,0,0,1];
    T_final=T_t*T_final;   %更新旋转矩阵
    disp(['误差err=',num2str(err)]);
    disp('旋转矩阵T=');
    disp(T_final);
     
    data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新点集
    if iteration>=200
        break
    end
end
 
disp(inv(T0));  %旋转矩阵真值
 
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b');
hold on;
scatter3(x2,y2,z2,'r');
hold off;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

参考：
文献：一种基于点云匹配的激光雷达/IMU联合标定方法
博客（点云提取）：https://blog.csdn.net/u014072827/article/details/110948996