激光雷达与组合贯导（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 ();
}

提取点云坐标

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

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

方法二：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;
}

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;

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