精华内容
下载资源
问答
  • Autoware 提供了一个 CalibrationToolKit 联合标定雷达和相机的工具,下面的链接记录了标定过程,建议使用这个工具来标定,...一、Camera-Lidar 联合标定 标定就是找到雷达到相机的空间转换关系,在不同的坐标系之间

    Autoware 提供了一个 CalibrationToolKit 联合标定雷达和相机的工具,下面的链接记录了标定过程,建议使用这个工具来标定,精度要高点:
    Autoware 工具 CalibrToolKit 标定 Robosense 雷达和 ZED 相机!
    Robosense-16 雷达和 ZED 双目相机的联合标定过程:
    Autoware 进行 Robosense-16 线雷达与 ZED 双目相机联合标定!
    一、Camera-Lidar 联合标定
    标定就是找到雷达到相机的空间转换关系,在不同的坐标系之间转换需要旋转矩阵 R 和平移矩阵 T,为后续的雷达和相机数据融合做准备:
    在这里插入图片描述
    Camera-LIDAR 联合标定分为 2 步:
    1.相机内参标定
    2.雷达相机外参标定
    1.1 Camera 内参标定方法
    ROS Camera Calibration Tools
    SLAM之相机标定
    1.2 Camera-Lidar 外参标定
    1.2.1 Autoware(推荐)

    激光雷达和相机的联合标定(Camera-LiDAR Calibration)之Autoware
    无人驾驶汽车系统入门(二十二)——使用Autoware实践激光雷达与摄像机组合标定
    使用Autoware进行(双目)相机与激光雷达的联合标定
    Camera-Lidar Calibration with Autoware
    视觉激光雷达信息融合与联合标定
    激光雷达(lidar)和相机(camera)联合标定调研(基于Autoware的详细步骤)
    多目相机、Velodyne标定那些破事
    无人驾驶-激光雷达与相机联合校准(Lidar Camera Calibration)
    1.2.2 lidar_camera_calibration
    lidar_camera_calibration
    lidar_camera_calibration项目——激光雷达和相机联合标定
    相机雷达联合标定
    无人驾驶-激光雷达与相机联合校准(Lidar Camera Calibration)
    使用autoware获得相机内参并与雷达联合标定:标定双目、外参矩阵求逆问题
    Autoware完整安装及联合标定工具箱安装
    Autoware搭建自动驾驶系统.一:数据记录/播放和传感器校准
    1.2.3 but_calibration_camera_velodyne
    but_calibration_camera_velodyne
    博客:but_calibration_camera_velodyne
    1.2.4 Apoll
    激光雷达和相机的联合标定(Camera-LiDAR Calibration)之apollo
    二、Lidar-IMU 联合标定
    lidar-imu 联合标定
    lidar-imu 联合标定开源项目
    多传感融合内外参标定(一):lidar-imu外参标定工具lidar_align
    三、Camera-IMU 联合标定
    利用kalibr工具进行camera-IMU标定
    kalibr
    四、毫米波雷达 - Camera 联合标定
    相机到毫米波雷达标定
    五、多相机联合标定
    kalibr
    六、多类型标定总结
    自动驾驶(二十六)---------传感器标定详解
    自动驾驶算法学习:多传感器信息融合(标定, 数据融合, 任务融合)

    展开全文
  • 雷达相机标定文件---lidar_cam_54_1.yml,已经标定完成,标定的数据是velodyne和相机
  • 这里介绍一个卡耐基梅隆大学开源的一款标定软件,使用离线方法,和单目和双目标定一样,直接再windows下面输入图片即可标定。 卡耐基梅隆源代码下载地址:代码+文档(需FQ) http://www.cs.cmu.edu/~ranjith/lcct....

    这里介绍一个卡耐基梅隆大学开源的一款标定软件,使用离线方法,和单目和双目标定一样,直接再windows下面输入图片即可标定。

    在这里插入图片描述

    卡耐基梅隆源代码下载地址:代码+文档(需FQ) http://www.cs.cmu.edu/~ranjith/lcct.html

    改进源代码下载地址:新增3D可视化操作 https://github.com/zhixy/Laser-Camera-Calibration-Toolbox

    注意事项:

    默认方向朝上,如果相反则需要更改。
    far和near是设置最远和最近距离,最好一次设置不要更改。
    其使用matlab进行内参标定,且是原始方法。如果使用matlab自带的calibrator得自己手动输入T和R 图像数量等参数。
    标定数据如果有太多噪声会导致很大误差,可以使用PCL去拟合平面再叠加回去。
    其中涉及bin pcd xyz 等文件格式的转换
    
    展开全文
  • 激光雷达lidar标定

    2021-03-25 13:11:21
    相机标定类似,激光雷达也有内参(由厂家提供)和外参之分 。 内参是内部激光发射器坐标囍和雷达自身坐标器之间的转换关系。外参是激光雷达与移动机器人之间的坐标系转换关系。我们主要标定的是俯仰角(x轴)和侧...

    与相机标定类似,激光雷达也有内参(由厂家提供)和外参之分 。

    内参

    内参是内部激光发射器坐标囍和雷达自身坐标器之间的转换关系。

    外参

    外参是激光雷达与其他坐标系(例如车辆后轴中心)之间的坐标系转换关系。我们主要标定的是俯仰角(x轴)和侧倾角(y轴)

    在这里插入图片描述

    理论基础1:使用长宽已知的矩形板ABCD来标定

    我们使用长宽已知的矩形板ABCD来标定 γ\gamma , 利用三角形余弦定理来求解侧倾角。

    几何关系求解 α\alpha

    在这里插入图片描述
    在这里插入图片描述

    理论基础2:多线雷达使用的是纸箱法

    1 相机与激光雷达的联合标定

    激光雷达与摄像头标定。比如说左上角,我看到凳子,左下角激光雷达也看到的是凳子,那么我通过标定的方式,得到转移矩阵,两个传感器得到的数据点是同一个物体。

    在这里插入图片描述

    在这里插入图片描述由相机捕获的图像数据由(U,V)表示,激光雷达捕获的3维点阵云用(X,Y,Z)表示,我们的目标是建立一个转化矩阵M,将3维点(x,y,z)映射到2维点(u,v),即:

    在这里插入图片描述矩阵(fu,fv,u0,v0)是相机参数,fu以及fv是XY轴方向尺度因子(水平方向和垂直方向的有效焦距),u0,v0是像平面(image plane)的中心点,又称主点坐标。R为旋转矩阵,t为平移矢量。
    在这里插入图片描述
    在这里插入图片描述

    在这里插入图片描述
    根据不同姿态下定标板平面,可得到一系列的线性方程,解得标定参数。

    其中:

    本文基于读者有一定的基础之后,单目标定结果为焦距fc 焦点cc 畸变系数k,双目标定结果为旋转+平移矩阵,联合标定求解的结果为3x3旋转矩阵+1x3平移矩阵,这种比较简单的流程都已经了解。

    具体求解方法较为复杂,其中最小二乘或L-M 多参数方程求解等,再matlab或者网上已经有开源代码,没必要自己开发。

    ros中的联合校准包

    这个包用于Velodyne激光雷达和相机联合校准,适用于VLP-16及以上,依赖于aruco_ros包和轻微修改的aruco_mapping包
    1)构建

    clone代码从https://github.com/ankitdhall/lidar_camera_calibration.git,把该代码仓库, dependencies/aruco_ros and dependencies/aruco_mapping 目录放到 path/to/your/ros/workspace/src , 执行下面的命令:

    catkin_make -DCATKIN_WHITELIST_PACKAGES="aruco_ros;aruco_mapping"
    catkin_make -DCATKIN_WHITELIST_PACKAGES=""
    

    如果,没有安装velodyne包需要先安装该包,make aruco_ros时首先要先安装aruco_msgs和aruco
    2)开始

    为了相机与激光雷达进行联合校准,两个配置文件需要进行修改,配置文件在lidar_camera_calibration/conf目录下。

    config_file.txt
    
        1280 720
        -2.5 2.5
        -4.0 4.0
        0.0 2.5
        0.05
        2
        0
        611.651245 0.0 642.388357 0.0
        0.0 688.443726 365.971718 0.0
        0.0 0.0 1.0 0.0
        1.57 -1.57 0.0
    

    文件格式:

        image_width image_height
        x- x+
        y- y+
        z- z+ //去除点阵云中不想要 的点
        cloud_intensity_threshold
        number_of_markers
        use_camera_info_topic? //是否使用camera_info topic
        fx 0 cx 0
        0 fy cy 0
        0 0 1 0
    
        MAX_ITERS
    
        initial_rot_x initial_rot_y initial_rot_z
    

    3)使用

    通过如下命令进行启动:

    roslaunch lidar_camera_calibration find_transform.launch
    

    https://blog.csdn.net/a2281965135/article/details/79785784

    MATLAB 相机与激光雷达的标定

    使用工具箱:

    在这里插入图片描述

    在这里插入图片描述

    在这里插入图片描述
    在这里插入图片描述

    在这里插入图片描述

    输出参数含义

    在这里插入图片描述

    在这里插入图片描述

    结果投影变换

    通过MATLAB标定结果做以下处理

    (1)获取相机内参数(径向畸变、切向畸变)
    k1,k2,k3,p1,p2
    (2)将R3×3矩阵转变为3×1矩阵(MATLAB中rotationMatrixToVector函数)

    在这里插入图片描述

    (3)将相关参数输入下面代码

    
    #include <opencv2/core/core.hpp>
    #include <opencv2/imgproc/imgproc.hpp>
    #include <opencv2/calib3d/calib3d.hpp>
    #include <opencv2/highgui/highgui.hpp>
    #include <opencv2/opencv.hpp>
    #include <pcl/point_types.h>
    #include <pcl/point_cloud.h>
    #include <pcl/io/pcd_io.h>
    #include <pcl/filters/filter.h>
    #include <pcl_ros/point_cloud.h>
    #include <ros/ros.h>
    #include <iostream>
    #include <string>
    using namespace std;
    using namespace cv;
    
    
    int main(int argc, char** argv)
    {
      // read a image and a pcd
      string imgfile="/home/fmc/Desktop/11/1/camera/*.jpg";//图片路径
      string pcdfile="/home/fmc/Desktop/11/1/lidar/*.pcd";//点云路径
      string savefile="/home/fmc/Desktop/11/1/save/";//投影结果保存路径
      vector<String> imgstr,pcdstr;
      glob(imgfile,imgstr);
      glob(pcdfile,pcdstr);
      if(imgstr.size()==0)
      {
        cerr<<"no img"<<endl;
        exit(-1);
      }
      if(pcdstr.size()==0)
      {
        cerr<<"no pcd"<<endl;
        exit(-1);
      }
      for(int i=1;i<=pcdstr.size();i++)
      {
        cv::Mat image_origin=imread(imgstr[i]);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_origin(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_withoutNAN(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::io::loadPCDFile<pcl::PointXYZI> (pcdstr[i], *cloud_origin);
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cloud_origin, *cloud_withoutNAN, indices);
        std::vector<cv::Point3f> pts_3d;
    
        for (size_t i = 0; i < cloud_withoutNAN->size(); ++i)
        {
          pcl::PointXYZI point_3d = cloud_withoutNAN->points[i];
          if (point_3d.x > 0)
          {
            pts_3d.emplace_back(cv::Point3f(point_3d.x, point_3d.y, point_3d.z));
          }
        }
        // read calibration parameter
        double fx = 2.6022e+03, fy = 2.6157e+03;
        double cx = 1.3147e+03, cy = 1.0051e+03;
        double k1 = -0.1222, k2 = 0.0046, k3 = 0;
        double p1 = 0, p2 = 0;
        cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0);
        cv::Mat distortion_coeff = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);
        cv::Mat r_vec = (cv::Mat_<double>(3, 1) << 1.2196,-1.1237,1.1942);
        cv::Mat t_vec = (cv::Mat_<double>(3, 1) << -0.3307, 0.0261, -0.0650);
    
        // project 3d-points into image view
        std::vector<cv::Point2f> pts_2d;
        cv::projectPoints(pts_3d, r_vec, t_vec, camera_matrix, distortion_coeff, pts_2d);
        cv::Mat image_project = image_origin.clone();
        int image_rows = image_origin.rows;
        int image_cols = image_origin.cols;
        // Point p0;
        // p0.x = pts_2d[10].x;
        // p0.y = pts_2d[10].y;
        for (size_t i = 0; i < pts_2d.size(); ++i)
        {
          cv::Point2f point_2d = pts_2d[i];
          if (point_2d.x > 0 && point_2d.x < image_cols && point_2d.y > 0 && point_2d.y < image_rows)
          {
            circle(image_project,point_2d,6,Scalar(0,0,255),6);
          }
          else
          {
            continue;
          }
        }
        image_project,COLOR_GRAY2BGR);
        ostringstream ostr;
        ostr<<savefile<<setfill('0')<<setw(4)<<i<<".jpg";
        cv::imwrite(ostr.str(),image_project);
        resize(image_project, image_project,Size(image_project.cols/4, image_project.rows/4));
        cv::imshow("project image", image_project);
        cv::waitKey(1);
      }
      return 0;
    }
    

    在这里插入图片描述

    (A)联合标定发展史

    • 使用点到点的标定方式

    这种方式原理较为简单,和单目内参标定类似,寻找关键的点进行一一匹配。

    之前看到一篇是多个点进行匹配的文章,由于公司不给上传,现文章已经无法找到,在这给出类似的文章:
    Extrinsic Calibration of a Camera and Laser Range Finder
    (下载:http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.80.7118&rep=rep1&type=pdf)

    雷达和相机外部参数关系

    • 3D点到点的求解方法:最小二乘法

    使用线到线的标定方式

    这种方法较为常用,其中KITTI数据集标定的方法与此类似,其增加了多张棋盘格,使用ransac软法进行匹配,原理基本一致。

    核心思想:

    a.通过单目标定,得到相机内参。

    b.通过内参计算标定板每个位置的外参矩阵,这里是针对相机的。

    c.通过雷达找到每个标定板的位置,得到法向量和距离(点到平面距离)。具体怎么得到参考PCL的平面拟合,文末参考文献会给出。

    d.通过线性结构得到初始解。

    e.通过L-M优化得到最终解。

    具体标定原理参考:A Comparative Analysis of Geometric and Image-Based Volumetric and Intensity Data Registration Algorithms (下载地址:https://pdfs.semanticscholar.org/ae17/2ddd58bbf7ba4aedce7bb99deab6e6017514.pdf)

    特征和差异性标定方式

    2 激光雷达与组合惯导联合标定

    https://blog.csdn.net/xx970829/article/details/115072158

    首先找到一个标志物,如一个大纸箱,确定某一个角为角点(由于采用的16线激光雷达,如果同时拾取4个角为角点,计算出来的误差是很大的,当然这里也可以自己写一个算法,来自动计算出纸箱上表面的中心点位置),假设这个标志物的在地图坐标系中的坐标为A(a,b,c),通过无人机搭载激光雷达和组合惯导来采集该标志物,通过点云可以看到该标志物,而且可以得到该标志物对应角点的在雷达坐标系中的坐标(x1,y1,z1),时间戳对齐后,可以提取出该帧对应的四元数(qw,qx,qy,qz)及gps经度,纬度,海拔。
    通过四元数可以算出相对地图坐标系的旋转矩阵,gps可以得到相对地图坐标系的平移,可以得到变换矩阵M1。

    在这里插入图片描述

    假设雷达坐标系到惯导坐标系的平移旋转矩阵为M0,那么标志物对应角点在地图坐标系中的位置就可以通过四元数和gps计算出来:

    在这里插入图片描述

    上图公式可知:(a,b,c)和M0(4x4)是不变的,M1和角点在雷达坐标系中的位置是可以知道的,那么如果知道多组(至少5组)数据,其实就可以求出(a,b,c)和M0(4x4)了。
    matlab程序

    程序1 :(靠特征点计算)

    [使用时,将提取到的数据保存到一个param.txt文件里,按四元数(w x y z)、经度、纬度、海拔、点云(x,y,z)的顺序保存,param.txt文件与这几个.m文件放在同一目录下。运行imu_lidar.m文件,将生成一个result.txt文件来保存结果]
    imu_lidar.m

    clc
    syms a b c t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
    
    m0=[0,0,0,0,0,1,0.1,0,1,0,0.1,1,0,0,0.2];
    x=fsolve(@fun,m0,optimset('fsolve'));
    a=x(1);   b=x(2);   c=x(3);
    t11=x(4); t12=x(5); t13=x(6); tx=x(7);
    t21=x(8); t22=x(9); t23=x(10);ty=x(11);
    t31=x(12);t32=x(13);t33=x(14);tz=x(15);
    
    R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
    q=dcm2quat(R);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                旋转矩阵                  ');
    fprintf('%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf('%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf('%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                  平移                   ');
    fprintf('%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                 四元数                  ');
    fprintf('%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    %----------------------------------------------------------------------------------------------------------
    T=fopen('.\result.txt','w');%创建文件
    fprintf(T,'旋转矩阵: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    fprintf(T,'平移: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    fprintf(T,'四元数: \n');
    fprintf('%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    fclose(T);
    
    
    
    
    function P=fun(x)
    
    %打开txt数据
    F=importdata('.\param.txt');
    
    %四元数转旋转矩阵                              
    R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);  
    R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);  
    R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);      
    R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);       
    R5=quat2dcm([F(5,1) F(5,2) F(5,3) F(5,4)]);     
    %经度           纬度          海拔      
    gx_1=F(1,5);  gy_1=F(1,6);  gz_1=F(1,7);  
    gx_2=F(2,5);  gy_2=F(2,6);  gz_2=F(2,7); 
    gx_3=F(3,5);  gy_3=F(3,6);  gz_3=F(3,7); 
    gx_4=F(4,5);  gy_4=F(4,6);  gz_4=F(4,7); 
    gx_5=F(5,5);  gy_5=F(5,6);  gz_5=F(5,7);
    % 点云
    x1=F(1,8);  y1=F(1,9);  z1=F(1,10);
    x2=F(2,8);  y2=F(2,9);  z2=F(2,10);
    x3=F(3,8);  y3=F(3,9);  z3=F(3,10); 
    x4=F(4,8);  y4=F(4,9);  z4=F(4,10);
    x5=F(5,8);  y5=F(5,9);  z5=F(5,10);
    %经纬度转墨卡托
    [gx11,gy11]=gpstoMercator(gx_1,gy_1);
    [gx22,gy22]=gpstoMercator(gx_2,gy_2);
    [gx33,gy33]=gpstoMercator(gx_3,gy_3);
    [gx44,gy44]=gpstoMercator(gx_4,gy_4);
    [gx55,gy55]=gpstoMercator(gx_5,gy_5);
    %纬度差为飞机的x方向位移,经度差为飞机的y方向位移
    gx1=0;          gy1=0;           gz1=0;
    gx2=gy11-gy22;  gy2=gx11-gx22;   gz2=gz_1-gz_2;
    gx3=gy11-gy33;  gy3=gx11-gx33;   gz3=gz_1-gz_3;
    gx4=gy11-gy44;  gy4=gx11-gx44;   gz4=gz_1-gz_4;
    gx5=gy11-gy55;  gy5=gx11-gx55;   gz5=gz_1-gz_5;
    %第一帧
    m1_11=R1(1,1);	  m1_12=R1(1,2);	m1_13=R1(1,3);
    m1_21=R1(2,1);    m1_22=R1(2,2);    m1_23=R1(2,3);
    m1_31=R1(3,1);    m1_32=R1(3,2);    m1_33=R1(3,3);
    %第2帧
    m2_11=R2(1,1);	  m2_12=R2(1,2);	m2_13=R2(1,3);
    m2_21=R2(2,1);    m2_22=R2(2,2);    m2_23=R2(2,3);
    m2_31=R2(3,1);    m2_32=R2(3,2);    m2_33=R2(3,3);
    %第3帧
    m3_11=R3(1,1);	  m3_12=R3(1,2);	m3_13=R3(1,3);
    m3_21=R3(2,1);    m3_22=R3(2,2);    m3_23=R3(2,3);
    m3_31=R3(3,1);    m3_32=R3(3,2);    m3_33=R3(3,3);
    %第4帧
    m4_11=R4(1,1);	  m4_12=R4(1,2);	m4_13=R4(1,3);
    m4_21=R4(2,1);    m4_22=R4(2,2);    m4_23=R4(2,3);
    m4_31=R4(3,1);    m4_32=R4(3,2);    m4_33=R4(3,3);
    %第5帧
    m5_11=R5(1,1);	  m5_12=R5(1,2);	m5_13=R5(1,3);
    m5_21=R5(2,1);    m5_22=R5(2,2);    m5_23=R5(2,3);
    m5_31=R5(3,1);    m5_32=R5(3,2);    m5_33=R5(3,3);
    
    %     t11=x(4)t12=x(5)t13=x(6);tx=x(7);    t21=x(8)t22=x(9)t23=x(10) ty=x(11);    t31=x(12)t32=x(13)t33=x(14);tz=x(15);
    P=[m1_11*(x(4)*x1+x(5)*y1+x(6)*z1+x(7))+m1_12*(x(8)*x1+x(9)*y1+x(10)*z1+x(11))+m1_13*(x(12)*x1+x(13)*y1+x(14)*z1+x(15))+gx1-x(1);
       m1_21*(x(4)*x1+x(5)*y1+x(6)*z1+x(7))+m1_22*(x(8)*x1+x(9)*y1+x(10)*z1+x(11))+m1_23*(x(12)*x1+x(13)*y1+x(14)*z1+x(15))+gy1-x(2);
       m1_31*(x(4)*x1+x(5)*y1+x(6)*z1+x(7))+m1_32*(x(8)*x1+x(9)*y1+x(10)*z1+x(11))+m1_33*(x(12)*x1+x(13)*y1+x(14)*z1+x(15))+gz1-x(3);
       m2_11*(x(4)*x2+x(5)*y2+x(6)*z2+x(7))+m2_12*(x(8)*x2+x(9)*y2+x(10)*z2+x(11))+m2_13*(x(12)*x2+x(13)*y2+x(14)*z2+x(15))+gx2-x(1);
       m2_21*(x(4)*x2+x(5)*y2+x(6)*z2+x(7))+m2_22*(x(8)*x2+x(9)*y2+x(10)*z2+x(11))+m2_23*(x(12)*x2+x(13)*y2+x(14)*z2+x(15))+gy2-x(2);
       m2_31*(x(4)*x2+x(5)*y2+x(6)*z2+x(7))+m2_32*(x(8)*x2+x(9)*y2+x(10)*z2+x(11))+m2_33*(x(12)*x2+x(13)*y2+x(14)*z2+x(15))+gz2-x(3);
       m3_11*(x(4)*x3+x(5)*y3+x(6)*z3+x(7))+m3_12*(x(8)*x3+x(9)*y3+x(10)*z3+x(11))+m3_13*(x(12)*x3+x(13)*y3+x(14)*z3+x(15))+gx3-x(1);
       m3_21*(x(4)*x3+x(5)*y3+x(6)*z3+x(7))+m3_22*(x(8)*x3+x(9)*y3+x(10)*z3+x(11))+m3_23*(x(12)*x3+x(13)*y3+x(14)*z3+x(15))+gy3-x(2);
       m3_31*(x(4)*x3+x(5)*y3+x(6)*z3+x(7))+m3_32*(x(8)*x3+x(9)*y3+x(10)*z3+x(11))+m3_33*(x(12)*x3+x(13)*y3+x(14)*z3+x(15))+gz3-x(3);
       m4_11*(x(4)*x4+x(5)*y4+x(6)*z4+x(7))+m4_12*(x(8)*x4+x(9)*y4+x(10)*z4+x(11))+m4_13*(x(12)*x4+x(13)*y4+x(14)*z4+x(15))+gx4-x(1);
       m4_21*(x(4)*x4+x(5)*y4+x(6)*z4+x(7))+m4_22*(x(8)*x4+x(9)*y4+x(10)*z4+x(11))+m4_23*(x(12)*x4+x(13)*y4+x(14)*z4+x(15))+gy4-x(2);
       m4_31*(x(4)*x4+x(5)*y4+x(6)*z4+x(7))+m4_32*(x(8)*x4+x(9)*y4+x(10)*z4+x(11))+m4_33*(x(12)*x4+x(13)*y4+x(14)*z4+x(15))+gz4-x(3);
       m5_11*(x(4)*x5+x(5)*y5+x(6)*z5+x(7))+m5_12*(x(8)*x5+x(9)*y5+x(10)*z5+x(11))+m5_13*(x(12)*x5+x(13)*y5+x(14)*z5+x(15))+gx5-x(1);
       m5_21*(x(4)*x5+x(5)*y5+x(6)*z5+x(7))+m5_22*(x(8)*x5+x(9)*y5+x(10)*z5+x(11))+m5_23*(x(12)*x5+x(13)*y5+x(14)*z5+x(15))+gy5-x(2);
       m5_31*(x(4)*x5+x(5)*y5+x(6)*z5+x(7))+m5_32*(x(8)*x5+x(9)*y5+x(10)*z5+x(11))+m5_33*(x(12)*x5+x(13)*y5+x(14)*z5+x(15))+gz5-x(3)];
    end
    
    
    function [jing,wei]  = gpstoMercator(j,w )
    jing = j * 20037508.34 / 180;
        ly = log(tand((90+ w)*pi/360))/(pi/180);
    wei =  ly *20037508.34/180;
    end
    
    

    在实际操作过程中,发现特征点不太好取,纸箱或小车在点云中的特征点都不太明显,于是提出了一个新的解决方案,通过提取雷达坐标系中点云的线特征来建立空间直线方程,然后取不同位姿下该线特征(可以是室外的楼梯或台阶)上的雷达点进行运算。

    程序2 :

    https://blog.csdn.net/xx970829/article/details/115098426
    按照方案一,在实际采集数据时比较困难,纸箱或车等标志物在点云中都不够明显,若要良好效果需要以一栋小平房的一角来做角点,于是提出了方案二:通过室外楼梯或台阶一条明显的点云线在地图坐标系中建立一条空间直线方程,在不同时刻,不同帧之间对应雷达坐标系中的点云线在地图坐标系中在同一条空间直线Ax+By+Cz+D=0上。
    然后通过无人机搭载激光雷达和组合惯导来采集该标志线,通过点云可以看到该标志线,而且可以得到该标志线对应角点的在雷达坐标系中的坐标(x1,y1,z1),时间戳对齐后,可以提取出该帧对应的四元数(qw,qx,qy,qz)及gps经度,纬度,海拔。
    通过四元数可以算出相对地图坐标系的旋转矩阵,gps可以得到相对地图坐标系的平移,可以得到变换矩阵M1。

    (下面的代码中只写了取四帧不同位姿下的点云,每帧4个雷达点)
    [使用时,将提取到的数据保存到一个param.txt文件里,按四元数(w x y z)、经度、纬度、海拔、点云(x,y,z)的顺序保存(每帧写4个点云坐标),param.txt文件与这几个.m文件放在同一目录下。运行imu_lidar.m文件,将生成一个result.txt文件来保存结果]

    clc
    syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
    
    m0=[0,0,0,0,0,1,0.1,0,1,0,0.1,1,0,0,0.2,0];
    x=fsolve(@funline,m0,optimset('fsolve'));
    a=x(1);   b=x(2);   c=x(3);
    t11=x(4); t12=x(5); t13=x(6); tx=x(7);
    t21=x(8); t22=x(9); t23=x(10);ty=x(11);
    t31=x(12);t32=x(13);t33=x(14);tz=x(15);
    
    R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
    q=dcm2quat(R);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                旋转矩阵                  ');
    fprintf('%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf('%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf('%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                  平移                   ');
    fprintf('%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                 四元数                  ');
    fprintf('%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    %----------------------------------------------------------------------------------------------------------
    T=fopen('.\result.txt','w');%创建文件
    fprintf(T,'旋转矩阵: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    fprintf(T,'平移: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    fprintf(T,'四元数: \n');
    fprintf('%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    fclose(T);
    
    
    
    function P=funline(x)
    
    %打开txt数据
    F=importdata('.\param.txt');
    
    %四元数转旋转矩阵                              
    R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
    R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);  
    R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);      
    R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);  
    %经度           纬度          海拔      
    gx_1=F(1,5);  gy_1=F(1,6);  gz_1=F(1,7);
    gx_2=F(2,5);  gy_2=F(2,6);  gz_2=F(2,7); 
    gx_3=F(3,5);  gy_3=F(3,6);  gz_3=F(3,7); 
    gx_4=F(4,5);  gy_4=F(4,6);  gz_4=F(4,7);
    % 点云
    x11=F(1,8);   y11=F(1,9);   z11=F(1,10);    x12=F(1,11);  y12=F(1,12);  z12=F(1,13);    x13=F(1,14);  y13=F(1,15);  z13=F(1,16);    x14=F(1,17);  y14=F(1,18);  z14=F(1,19);
    x21=F(2,8);   y21=F(2,9);   z21=F(2,10);    x22=F(2,11);  y22=F(2,12);  z22=F(2,13);    x23=F(2,14);  y23=F(2,15);  z23=F(2,16);    x24=F(2,17);  y24=F(2,18);  z24=F(2,19);
    x31=F(3,8);   y31=F(3,9);   z31=F(3,10);    x32=F(3,11);  y32=F(3,12);  z32=F(3,13);    x33=F(3,14);  y33=F(3,15);  z33=F(3,16);    x34=F(3,17);  y34=F(3,18);  z34=F(3,19);
    x41=F(4,8);   y41=F(4,9);   z41=F(4,10);    x42=F(4,11);  y42=F(4,12);  z42=F(4,13);    x43=F(4,14);  y43=F(4,15);  z43=F(4,16);    x44=F(4,17);  y44=F(4,18);  z44=F(4,19);
    %经纬度转墨卡托
    [gx11,gy11]=gpstoMercator(gx_1,gy_1);
    [gx22,gy22]=gpstoMercator(gx_2,gy_2);
    [gx33,gy33]=gpstoMercator(gx_3,gy_3);
    [gx44,gy44]=gpstoMercator(gx_4,gy_4);
    %纬度差为飞机的x方向位移,经度差为飞机的y方向位移
    gx1=0;          gy1=0;           gz1=0;
    gx2=gy11-gy22;  gy2=gx11-gx22;   gz2=gz_1-gz_2;
    gx3=gy11-gy33;  gy3=gx11-gx33;   gz3=gz_1-gz_3;
    gx4=gy11-gy44;  gy4=gx11-gx44;   gz4=gz_1-gz_4;
    %第一帧旋转赋值
    m1_11=R1(1,1);	  m1_12=R1(1,2);	m1_13=R1(1,3);
    m1_21=R1(2,1);    m1_22=R1(2,2);    m1_23=R1(2,3);
    m1_31=R1(3,1);    m1_32=R1(3,2);    m1_33=R1(3,3);
    %第2帧旋转赋值
    m2_11=R2(1,1);	  m2_12=R2(1,2);	m2_13=R2(1,3);
    m2_21=R2(2,1);    m2_22=R2(2,2);    m2_23=R2(2,3);
    m2_31=R2(3,1);    m2_32=R2(3,2);    m2_33=R2(3,3);
    %第3帧旋转赋值
    m3_11=R3(1,1);	  m3_12=R3(1,2);	m3_13=R3(1,3);
    m3_21=R3(2,1);    m3_22=R3(2,2);    m3_23=R3(2,3);
    m3_31=R3(3,1);    m3_32=R3(3,2);    m3_33=R3(3,3);
    %第4帧旋转赋值
    m4_11=R4(1,1);	  m4_12=R4(1,2);	m4_13=R4(1,3);
    m4_21=R4(2,1);    m4_22=R4(2,2);    m4_23=R4(2,3);
    m4_31=R4(3,1);    m4_32=R4(3,2);    m4_33=R4(3,3);
     
    
    %     t11=x(4)t12=x(5)t13=x(6);tx=x(7);    t21=x(8)t22=x(9)t23=x(10) ty=x(11);    t31=x(12)t32=x(13)t33=x(14);tz=x(15);
    %                      c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c---------------------------------c--------c--------c---------------------c---------c--------c-----------------------c----------c---------c-------------------------------c--------c--------c---------------------c-------c---------c------------------------c--------c---------c--------------------
    P=[ x(1)*(m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1)+x(2)*(m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1)+x(3)*(m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1)+x(16);
        x(1)*(m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1)+x(2)*(m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1)+x(3)*(m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1)+x(16);
        x(1)*(m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1)+x(2)*(m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1)+x(3)*(m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1)+x(16);
        x(1)*(m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1)+x(2)*(m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1)+x(3)*(m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1)+x(16);
        x(1)*(m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2)+x(2)*(m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2)+x(3)*(m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2)+x(16);
        x(1)*(m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2)+x(2)*(m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2)+x(3)*(m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2)+x(16);
        x(1)*(m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2)+x(2)*(m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2)+x(3)*(m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2)+x(16);
        x(1)*(m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2)+x(2)*(m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2)+x(3)*(m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2)+x(16);
        x(1)*(m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3)+x(2)*(m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3)+x(3)*(m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3)+x(16);
        x(1)*(m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3)+x(2)*(m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3)+x(3)*(m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3)+x(16);
        x(1)*(m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3)+x(2)*(m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3)+x(3)*(m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3)+x(16);
        x(1)*(m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3)+x(2)*(m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3)+x(3)*(m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3)+x(16);
        x(1)*(m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4)+x(2)*(m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4)+x(3)*(m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4)+x(16);
        x(1)*(m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4)+x(2)*(m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4)+x(3)*(m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4)+x(16);
        x(1)*(m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4)+x(2)*(m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4)+x(3)*(m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4)+x(16);
        x(1)*(m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4)+x(2)*(m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4)+x(3)*(m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4)+x(16)];
    end
    
    
    
    function [jing,wei]  = gpstoMercator(j,w )
    jing = j * 20037508.34 / 180;
        ly = log(tand((90+ w)*pi/360))/(pi/180);
    wei =  ly *20037508.34/180;
    end
    
    

    对程序的改进

    imu_lidar_fmincon.m

    clc
    syms a b c d t11 t12 t13 tx t21 t22 t23 ty t31 t32 t33 tz;
    %**********************************************************************************************
    %                   输入参数
    %*********************************************************************************************
    %设初始值
    imux=0;  %空间点数据在惯导坐标系的大致位置
    imuy=0.5;
    imuz=0.75;
    txx=0.18;%手测位移
    
    tyy=0;
    tzz=0.24;
    l_i_z=0.2;%手测沿惯导坐标系z轴平移
    l_i_x=0.1;%手测沿惯导坐标系x轴平移
    l_i_z2=0.14;%手测沿惯导坐标系z轴平移(到雷达中心高度)
    aa=40;%沿惯导坐标系y轴旋转角度
    %--------------------------------------------------------------------------
    [A,B,C,D]= funintial(imux,imuy,imuz);
    h=0;
    %         [        |        |       |       |     
    m0=[A,B,C,h,h,1,txx,h,1,h,tyy,1,h,h,tzz,D];
    A = [];b = [];
    Aeq = []; beq = [];
    vlb = []; vub = [];
    options = optimoptions('fmincon','Algorithm','sqp','Display','iter','ConstraintTolerance',1e-12);
    [x,fval]=fmincon(@funcon,m0,A, b, Aeq, beq, vlb, vub,@fcon,options);
    Rr=imutolidar(l_i_z,l_i_x,l_i_z2,aa);
    a=x(1);   b=x(2);   c=x(3);
    t11=x(4); t12=x(5); t13=x(6); tx=x(7);
    t21=x(8); t22=x(9); t23=x(10);ty=x(11);
    t31=x(12);t32=x(13);t33=x(14);tz=x(15);
    
    R=[t11,t12,t13;t21,t22,t23;t31,t32,t33];
    q=dcm2quat(R);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                原始矩阵                  ');
    disp(Rr);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                旋转矩阵                  ');
    fprintf('%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf('%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf('%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                  平移                   ');
    fprintf('%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    disp('/*------------------------------------------------------------------------------------------------');
    disp('/*                 四元数                  ');
    fprintf('%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    %----------------------------------------------------------------------------------------------------------
    T=fopen('.\result.txt','w');%创建文件
    fprintf(T,'旋转矩阵: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t11,t12,t13);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t21,t22,t23);
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',t31,t32,t33);
    fprintf(T,'平移: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f \n',tx,ty,tz);
    fprintf(T,'四元数: \n');
    fprintf(T,'%6.8f  %6.8f  %6.8f   %6.8f \n',q(1,1),q(1,2),q(1,3),q(1,4));
    fclose(T);
    
    
    
    %***********************************************************
    %            约束条件
    %***********************************************************
    
    function [c,ceq]=fcon(x) %构造约束函数 c=[]<0; ceq= 0
    %-----------------------------------------------------------
    %                   输入参数
    %-----------------------------------------------------------
    tx=0.18;     %手测位移
    ty=0;
    tz=0.24;
    l_i_z=0.2;   %手测沿惯导坐标系z轴平移
    l_i_x=0.1;   %手测沿惯导坐标系x轴平移
    l_i_z2=0.14; %手测沿惯导坐标系z轴平移(到雷达中心高度)
    a=40;        %手测沿惯导坐标系y轴旋转角度
    %----------------------------------
    L=0.3;       %点偏离平面范围(单位m)
    Lt=0.05;     %偏离手测位移
    Lr=0.05;     %偏离旋转
    La=50;       %偏离平面系数范围
    %-----------------------------------
    imux=0;      %算空间平面时空间点数据在惯导坐标系的大致位置
    imuy=0.5;
    imuz=0.75;
    %----------------------------------
    R=imutolidar(l_i_z,l_i_x,l_i_z2,a);
    [A,B,C,D]= funintial(imux,imuy,imuz);
    %-----------------------------------
    %打开txt数据
    F=importdata('.\data.txt');
    
    %四元数转旋转矩阵                              
    R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
    R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);  
    R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);      
    R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);  
    %经度           纬度          海拔      
    gx_1=F(1,5);  gy_1=F(1,6);  gz_1=F(1,7);
    gx_2=F(2,5);  gy_2=F(2,6);  gz_2=F(2,7); 
    gx_3=F(3,5);  gy_3=F(3,6);  gz_3=F(3,7); 
    gx_4=F(4,5);  gy_4=F(4,6);  gz_4=F(4,7);
    % 点云
    x11=F(1,8);   y11=F(1,9);   z11=F(1,10);    x12=F(1,11);  y12=F(1,12);  z12=F(1,13);    x13=F(1,14);  y13=F(1,15);  z13=F(1,16);    x14=F(1,17);  y14=F(1,18);  z14=F(1,19);
    x21=F(2,8);   y21=F(2,9);   z21=F(2,10);    x22=F(2,11);  y22=F(2,12);  z22=F(2,13);    x23=F(2,14);  y23=F(2,15);  z23=F(2,16);    x24=F(2,17);  y24=F(2,18);  z24=F(2,19);
    x31=F(3,8);   y31=F(3,9);   z31=F(3,10);    x32=F(3,11);  y32=F(3,12);  z32=F(3,13);    x33=F(3,14);  y33=F(3,15);  z33=F(3,16);    x34=F(3,17);  y34=F(3,18);  z34=F(3,19);
    x41=F(4,8);   y41=F(4,9);   z41=F(4,10);    x42=F(4,11);  y42=F(4,12);  z42=F(4,13);    x43=F(4,14);  y43=F(4,15);  z43=F(4,16);    x44=F(4,17);  y44=F(4,18);  z44=F(4,19);
    %经纬度转墨卡托
    [gx11,gy11]=gpstoMercator(gx_1,gy_1);
    [gx22,gy22]=gpstoMercator(gx_2,gy_2);
    [gx33,gy33]=gpstoMercator(gx_3,gy_3);
    [gx44,gy44]=gpstoMercator(gx_4,gy_4);
    %纬度差为飞机的x方向位移,经度差为飞机的y方向位移
    gx1=0;          gy1=0;           gz1=0;
    gx2=gy22-gy11;  gy2=gx22-gx11;   gz2=gz_2-gz_1;
    gx3=gy33-gy11;  gy3=gx33-gx11;   gz3=gz_3-gz_1;
    gx4=gy44-gy11;  gy4=gx44-gx11;   gz4=gz_4-gz_1;
    %第一帧旋转赋值
    m1_11=R1(1,1);	  m1_12=R1(1,2);	m1_13=R1(1,3);
    m1_21=R1(2,1);    m1_22=R1(2,2);    m1_23=R1(2,3);
    m1_31=R1(3,1);    m1_32=R1(3,2);    m1_33=R1(3,3);
    %第2帧旋转赋值
    m2_11=R2(1,1);	  m2_12=R2(1,2);	m2_13=R2(1,3);
    m2_21=R2(2,1);    m2_22=R2(2,2);    m2_23=R2(2,3);
    m2_31=R2(3,1);    m2_32=R2(3,2);    m2_33=R2(3,3);
    %第3帧旋转赋值
    m3_11=R3(1,1);	  m3_12=R3(1,2);	m3_13=R3(1,3);
    m3_21=R3(2,1);    m3_22=R3(2,2);    m3_23=R3(2,3);
    m3_31=R3(3,1);    m3_32=R3(3,2);    m3_33=R3(3,3);
    %第4帧旋转赋值
    m4_11=R4(1,1);	  m4_12=R4(1,2);	m4_13=R4(1,3);
    m4_21=R4(2,1);    m4_22=R4(2,2);    m4_23=R4(2,3);
    m4_31=R4(3,1);    m4_32=R4(3,2);    m4_33=R4(3,3);
    
    f1=x(1)*(m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1)+x(2)*(m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1)+x(3)*(m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1)+x(16);
    f2=x(1)*(m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1)+x(2)*(m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1)+x(3)*(m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1)+x(16);
    f3=x(1)*(m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1)+x(2)*(m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1)+x(3)*(m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1)+x(16);
    f4=x(1)*(m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1)+x(2)*(m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1)+x(3)*(m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1)+x(16);
    f5=x(1)*(m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2)+x(2)*(m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2)+x(3)*(m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2)+x(16);
    f6=x(1)*(m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2)+x(2)*(m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2)+x(3)*(m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2)+x(16);
    f7=x(1)*(m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2)+x(2)*(m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2)+x(3)*(m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2)+x(16);
    f8=x(1)*(m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2)+x(2)*(m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2)+x(3)*(m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2)+x(16);
    
    f9=x(1)*(m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3)+x(2)*(m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3)+x(3)*(m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3)+x(16);
    f10=x(1)*(m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3)+x(2)*(m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3)+x(3)*(m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3)+x(16);
    f11=x(1)*(m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3)+x(2)*(m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3)+x(3)*(m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3)+x(16);
    f12=x(1)*(m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3)+x(2)*(m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3)+x(3)*(m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3)+x(16);
    f13=x(1)*(m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4)+x(2)*(m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4)+x(3)*(m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4)+x(16);
    f14=x(1)*(m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4)+x(2)*(m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4)+x(3)*(m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4)+x(16);
    f15=x(1)*(m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4)+x(2)*(m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4)+x(3)*(m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4)+x(16);
    f16=x(1)*(m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4)+x(2)*(m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4)+x(3)*(m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4)+x(16);
    
    c=[x(1)-A-La;x(2)-B-La;x(3)-C-La;x(4)-R(1,1)-Lr;x(5)-R(1,2)-Lr;x(6)-R(1,3)-Lr;x(7)-tx-Lt;x(8)-R(2,1)-Lr;x(9)-R(2,2)-Lr;x(10)-R(2,3)-Lr;x(11)-ty-Lt;x(12)-R(3,1)-Lr;x(13)-R(3,2)-Lr;x(14)-R(3,3)-Lr;x(15)-tz-Lt;x(16)-D-La;-x(1)+A-La;-x(2)+B-La;-x(3)+C-La;-x(4)+R(1,1)-Lr;-x(5)+R(1,2)-Lr;-x(6)+R(1,3)-Lr;-x(7)+tx-Lt;-x(8)+R(2,1)-Lr;-x(9)+R(2,2)-Lr;-x(10)+R(2,3)-Lr;-x(11)+ty-Lt;-x(12)+R(3,1)-Lr;-x(13)+R(3,2)-Lr;-x(14)+R(3,3)-Lr;-x(15)+tz-Lt;-x(16)+D-La;f1-L;f2-L;f3-L;f4-L;f5-L;f6-L;f7-L;f8-L;f9-L;f10-L;f11-L;f12-L;f13-L;f14-L;f15-L;f16-L;L-f1;L-f2;L-f3;L-f4;L-f5;L-f6;L-f7;L-f8;L-f9;L-f10;L-f11;L-f12;L-f13;L-f14;L-f15;L-f16];
    ceq=[];
    end
    
    
    %***********************************************************
    %            目标函数
    %**********************************************************
    function F=funcon(x) %构造约束函数 c=[]<0; ceq= 0
    
    %打开txt数据
    F=importdata('.\data.txt');
    
    %四元数转旋转矩阵                              
    R1=quat2dcm([F(1,1) F(1,2) F(1,3) F(1,4)]);
    R2=quat2dcm([F(2,1) F(2,2) F(2,3) F(2,4)]);  
    R3=quat2dcm([F(3,1) F(3,2) F(3,3) F(3,4)]);      
    R4=quat2dcm([F(4,1) F(4,2) F(4,3) F(4,4)]);  
    %经度           纬度          海拔      
    gx_1=F(1,5);  gy_1=F(1,6);  gz_1=F(1,7);
    gx_2=F(2,5);  gy_2=F(2,6);  gz_2=F(2,7); 
    gx_3=F(3,5);  gy_3=F(3,6);  gz_3=F(3,7); 
    gx_4=F(4,5);  gy_4=F(4,6);  gz_4=F(4,7);
    % 点云
    x11=F(1,8);   y11=F(1,9);   z11=F(1,10);    x12=F(1,11);  y12=F(1,12);  z12=F(1,13);    x13=F(1,14);  y13=F(1,15);  z13=F(1,16);    x14=F(1,17);  y14=F(1,18);  z14=F(1,19);
    x21=F(2,8);   y21=F(2,9);   z21=F(2,10);    x22=F(2,11);  y22=F(2,12);  z22=F(2,13);    x23=F(2,14);  y23=F(2,15);  z23=F(2,16);    x24=F(2,17);  y24=F(2,18);  z24=F(2,19);
    x31=F(3,8);   y31=F(3,9);   z31=F(3,10);    x32=F(3,11);  y32=F(3,12);  z32=F(3,13);    x33=F(3,14);  y33=F(3,15);  z33=F(3,16);    x34=F(3,17);  y34=F(3,18);  z34=F(3,19);
    x41=F(4,8);   y41=F(4,9);   z41=F(4,10);    x42=F(4,11);  y42=F(4,12);  z42=F(4,13);    x43=F(4,14);  y43=F(4,15);  z43=F(4,16);    x44=F(4,17);  y44=F(4,18);  z44=F(4,19);
    %经纬度转墨卡托
    [gx11,gy11]=gpstoMercator(gx_1,gy_1);
    [gx22,gy22]=gpstoMercator(gx_2,gy_2);
    [gx33,gy33]=gpstoMercator(gx_3,gy_3);
    [gx44,gy44]=gpstoMercator(gx_4,gy_4);
    %纬度差为飞机的x方向位移,经度差为飞机的y方向位移
    gx1=0;          gy1=0;           gz1=0;
    gx2=gy22-gy11;  gy2=gx22-gx11;   gz2=gz_2-gz_1;
    gx3=gy33-gy11;  gy3=gx33-gx11;   gz3=gz_3-gz_1;
    gx4=gy44-gy11;  gy4=gx44-gx11;   gz4=gz_4-gz_1;
    
    %第一帧旋转赋值
    m1_11=R1(1,1);	  m1_12=R1(1,2);	m1_13=R1(1,3);
    m1_21=R1(2,1);    m1_22=R1(2,2);    m1_23=R1(2,3);
    m1_31=R1(3,1);    m1_32=R1(3,2);    m1_33=R1(3,3);
    %第2帧旋转赋值
    m2_11=R2(1,1);	  m2_12=R2(1,2);	m2_13=R2(1,3);
    m2_21=R2(2,1);    m2_22=R2(2,2);    m2_23=R2(2,3);
    m2_31=R2(3,1);    m2_32=R2(3,2);    m2_33=R2(3,3);
    %第3帧旋转赋值
    m3_11=R3(1,1);	  m3_12=R3(1,2);	m3_13=R3(1,3);
    m3_21=R3(2,1);    m3_22=R3(2,2);    m3_23=R3(2,3);
    m3_31=R3(3,1);    m3_32=R3(3,2);    m3_33=R3(3,3);
    %第4帧旋转赋值
    m4_11=R4(1,1);	  m4_12=R4(1,2);	m4_13=R4(1,3);
    m4_21=R4(2,1);    m4_22=R4(2,2);    m4_23=R4(2,3);
    m4_31=R4(3,1);    m4_32=R4(3,2);    m4_33=R4(3,3);
    
    %点坐标
    f1x=m1_11*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_12*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_13*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gx1;
    f1y=m1_21*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_22*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_23*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gy1;
    f1z=m1_31*(x(4)*x11+x(5)*y11+x(6)*z11+x(7))+m1_32*(x(8)*x11+x(9)*y11+x(10)*z11+x(11))+m1_33*(x(12)*x11+x(13)*y11+x(14)*z11+x(15))+gz1;
       
    f2x=m1_11*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_12*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_13*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gx1;
    f2y=m1_21*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_22*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_23*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gy1;
    f2z=m1_31*(x(4)*x12+x(5)*y12+x(6)*z12+x(7))+m1_32*(x(8)*x12+x(9)*y12+x(10)*z12+x(11))+m1_33*(x(12)*x12+x(13)*y12+x(14)*z12+x(15))+gz1;
    
    f3x=m1_11*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_12*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_13*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gx1;
    f3y=m1_21*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_22*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_23*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gy1;
    f3z=m1_31*(x(4)*x13+x(5)*y13+x(6)*z13+x(7))+m1_32*(x(8)*x13+x(9)*y13+x(10)*z13+x(11))+m1_33*(x(12)*x13+x(13)*y13+x(14)*z13+x(15))+gz1;
    
    f4x=m1_11*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_12*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_13*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gx1;
    f4y=m1_21*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_22*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_23*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gy1;
    f4z=m1_31*(x(4)*x14+x(5)*y14+x(6)*z14+x(7))+m1_32*(x(8)*x14+x(9)*y14+x(10)*z14+x(11))+m1_33*(x(12)*x14+x(13)*y14+x(14)*z14+x(15))+gz1;
    
    f5x=m2_11*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_12*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_13*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gx2;
    f5y=m2_21*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_22*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_23*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gy2;
    f5z=m2_31*(x(4)*x21+x(5)*y21+x(6)*z21+x(7))+m2_32*(x(8)*x21+x(9)*y21+x(10)*z21+x(11))+m2_33*(x(12)*x21+x(13)*y21+x(14)*z21+x(15))+gz2;
    
    f6x=m2_11*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_12*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_13*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gx2;
    f6y=m2_21*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_22*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_23*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gy2;
    f6z=m2_31*(x(4)*x22+x(5)*y22+x(6)*z22+x(7))+m2_32*(x(8)*x22+x(9)*y22+x(10)*z22+x(11))+m2_33*(x(12)*x22+x(13)*y22+x(14)*z22+x(15))+gz2;
    
    f7x=m2_11*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_12*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_13*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gx2;
    f7y=m2_21*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_22*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_23*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gy2;
    f7z=m2_31*(x(4)*x23+x(5)*y23+x(6)*z23+x(7))+m2_32*(x(8)*x23+x(9)*y23+x(10)*z23+x(11))+m2_33*(x(12)*x23+x(13)*y23+x(14)*z23+x(15))+gz2;
    
    f8x=m2_11*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_12*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_13*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gx2;
    f8y=m2_21*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_22*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_23*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gy2;
    f8z=m2_31*(x(4)*x24+x(5)*y24+x(6)*z24+x(7))+m2_32*(x(8)*x24+x(9)*y24+x(10)*z24+x(11))+m2_33*(x(12)*x24+x(13)*y24+x(14)*z24+x(15))+gz2;
    
    f9x=m3_11*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_12*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_13*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gx3;
    f9y=m3_21*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_22*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_23*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gy3;
    f9z=m3_31*(x(4)*x31+x(5)*y31+x(6)*z31+x(7))+m3_32*(x(8)*x31+x(9)*y31+x(10)*z31+x(11))+m3_33*(x(12)*x31+x(13)*y31+x(14)*z31+x(15))+gz3;
    
    f10x=m3_11*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_12*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_13*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gx3;
    f10y=m3_21*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_22*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_23*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gy3;
    f10z=m3_31*(x(4)*x32+x(5)*y32+x(6)*z32+x(7))+m3_32*(x(8)*x32+x(9)*y32+x(10)*z32+x(11))+m3_33*(x(12)*x32+x(13)*y32+x(14)*z32+x(15))+gz3;
    
    f11x=m3_11*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_12*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_13*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gx3;
    f11y=m3_21*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_22*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_23*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gy3;
    f11z=m3_31*(x(4)*x33+x(5)*y33+x(6)*z33+x(7))+m3_32*(x(8)*x33+x(9)*y33+x(10)*z33+x(11))+m3_33*(x(12)*x33+x(13)*y33+x(14)*z33+x(15))+gz3;
    
    f12x=m3_11*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_12*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_13*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gx3;
    f12y=m3_21*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_22*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_23*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gy3;
    f12z=m3_31*(x(4)*x34+x(5)*y34+x(6)*z34+x(7))+m3_32*(x(8)*x34+x(9)*y34+x(10)*z34+x(11))+m3_33*(x(12)*x34+x(13)*y34+x(14)*z34+x(15))+gz3;
    
    f13x=m4_11*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_12*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_13*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gx4;
    f13y=m4_21*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_22*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_23*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gy4;
    f13z=m4_31*(x(4)*x41+x(5)*y41+x(6)*z41+x(7))+m4_32*(x(8)*x41+x(9)*y41+x(10)*z41+x(11))+m4_33*(x(12)*x41+x(13)*y41+x(14)*z41+x(15))+gz4;
    
    f14x=m4_11*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_12*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_13*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gx4;
    f14y=m4_21*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_22*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_23*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gy4;
    f14z=m4_31*(x(4)*x42+x(5)*y42+x(6)*z42+x(7))+m4_32*(x(8)*x42+x(9)*y42+x(10)*z42+x(11))+m4_33*(x(12)*x42+x(13)*y42+x(14)*z42+x(15))+gz4;
    
    f15x=m4_11*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_12*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_13*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gx4;
    f15y=m4_21*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_22*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_23*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gy4;
    f15z=m4_31*(x(4)*x43+x(5)*y43+x(6)*z43+x(7))+m4_32*(x(8)*x43+x(9)*y43+x(10)*z43+x(11))+m4_33*(x(12)*x43+x(13)*y43+x(14)*z43+x(15))+gz4;
    
    f16x=m4_11*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_12*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_13*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gx4;
    f16y=m4_21*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_22*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_23*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gy4;
    f16z=m4_31*(x(4)*x44+x(5)*y44+x(6)*z44+x(7))+m4_32*(x(8)*x44+x(9)*y44+x(10)*z44+x(11))+m4_33*(x(12)*x44+x(13)*y44+x(14)*z44+x(15))+gz4;
    % d=|ax0+by0+cz0+d|/√(a?+b?+c?)
    d1=abs(f1x*x(1)+f1y*x(2)+f1z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d2=abs(f2x*x(1)+f2y*x(2)+f2z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d3=abs(f3x*x(1)+f3y*x(2)+f3z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d4=abs(f4x*x(1)+f4y*x(2)+f4z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d5=abs(f5x*x(1)+f5y*x(2)+f5z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d6=abs(f6x*x(1)+f6y*x(2)+f6z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d7=abs(f7x*x(1)+f7y*x(2)+f7z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d8=abs(f8x*x(1)+f8y*x(2)+f8z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d9=abs(f9x*x(1)+f9y*x(2)+f9z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d10=abs(f10x*x(1)+f10y*x(2)+f10z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d11=abs(f11x*x(1)+f11y*x(2)+f11z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d12=abs(f12x*x(1)+f12y*x(2)+f12z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d13=abs(f13x*x(1)+f13y*x(2)+f13z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d14=abs(f14x*x(1)+f14y*x(2)+f14z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d15=abs(f15x*x(1)+f15y*x(2)+f15z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    d16=abs(f16x*x(1)+f16y*x(2)+f16z*x(3)+x(16))/sqrt(x(1)^2+x(2)^2+x(3)^2);
    
    F=d1+d2+d3+d4+d5+d6+d7+d8+d9+d10+d11+d12+d13+d14+d15+d16;
    
    %-------------------------------------------------------------------------------------------------------------------------------------------------
    
    end
    
    
    %***********************************************************
    %            惯导坐标系到雷达坐标系
    %**********************************************************
    function R=imutolidar(l_i_z,l_i_x,l_i_z2,a)
    
    %沿惯导坐标系z轴平移l_i_zm
    t_z=[1 0 0 0;0 1 0 0;0 0 1 l_i_z;0 0 0 1];
    %绕惯导坐标系y轴逆时针旋转a度
    r_y=[cosd(a) 0 sind(a) 0;0 1 0 0;-sind(a) 0 cosd(a) 0;0 0 0 1];
    %沿惯导坐标系x轴平移l_i_x
    t_x=[1 0 0 l_i_x;0 1 0 0;0 0 1 l_i_z2;0 0 0 1];
    %绕惯导坐标系y轴逆时针旋转180度,然后绕z轴逆时针旋转90度 
     r_i_l=[0 0 1 0;0 -1 0 0;1 0 1 0;0 0 0 1];
    R=t_z*r_y*t_x*r_i_l;
     
    end
    
    
    

    3 多激光雷达外参自动标定算法

    https://github.com/Livox-SDK/Livox_automatic_calibration

    (Target-Free Automatic Calibration)并在Github上开源。该技术主要依靠几何一致性假设,即多个雷达扫描出来的局部三维模型是一致的,通过对基准雷达(LiDAR0)进行移动建图,然后将其余雷达数据对LiDAR0的重建地图不断进行迭代配准与计算,依靠一致性假设不断减少匹配误差,直到算法收敛并且满足标定矩阵刚性不变特性(六条平行线),最后用一致性算法得出最终标定矩阵(外参)。

    在这里插入图片描述

    展开全文
  • 该软件是基于高清矢量地图的LiDAR相机校准工具,可用于校准几乎没有手动标记的对应关系的非重叠LiDAR相机对,并检查校准结果的质量。 校准工具的重要功能: 以高清矢量图作为标定场的LiDAR摄像机标定和检查 推荐...
  • 一、总体标定步骤 标定就是找到雷达到相机的空间转换关系,在不同的坐标系之间转换需要旋转...SLAM之相机标定 外参标定:lidar_camera_calibration lidar_camera_calibration lidar_camera_calibration项目——激光

    一、总体标定步骤

    标定就是找到雷达到相机的空间转换关系,在不同的坐标系之间转换需要旋转矩阵 R 和平移矩阵 T,为后续的雷达和相机数据融合做准备:

    Camera-LIDAR 联合标定分为 2 步:

    1. 相机内参标定
    2. 雷达相机外参标定

    相机内参标定方法

    外参标定:lidar_camera_calibration

    外参标定:but_calibration_camera_velodyne

    外参标定:Autoware

    外参标定:Apoll

    二、其他传感器联合标定方法

    2.1 LIDAR-IMU 联合标定

    2.2 Camera-IMU 联合标定

    展开全文
  • 激光雷达与相机标定

    千次阅读 2020-10-28 13:29:57
    文章目录激光雷达与相机标定1.下载源码并编译2. 制作标定板3.配置4. 启动5. 过程操作遇到问题参考链接 激光雷达与相机标定 激光雷达与相机的标定方法有:Autoware, apollo, lidar_camera_calibration, 与but_...
  • 两个单线lidar位置的标定方法

    千次阅读 2018-07-04 23:34:05
    数据采集的时候,是用一个球用来标定位置关系和旋转关系,两个lidar同时扫描一个球的面,通过拟合的方法可以得出两个lidar分别对应的球心位置,根据采集数据的时间戳将两个lidar在同一时间戳采集的数据联系起来,...
  • 简单记录一下使用Autoware对lidar和cam联合标定的步骤和一些注意事项。 首先,开源的lidar和cam标定方案不多,花了一天查资料大概有以下几个: but_velodyne https://github.com/robofit/but_velodyne ...
  • 激光雷达和相机都有自己特有的优势,然而它们也各有缺点和不足,现在主流的感知算法都向传感器融合方法靠拢,这就涉及到不同传感器之间的标定问题,本博文主要针对一个开源项目讲解激光雷达和相机之间的外参标定,即...
  • camera_calibration 相机标定 vs工程
  • 综述 | 相机标定方法

    千次阅读 2019-08-28 19:33:04
    本文作者蔡量力,公众号:计算机视觉life成员,由于格式原因,公式显示可能出问题,建议阅读原文链接:综述 | 相机标定方法另外推荐几个原创的号 计算机视觉,Python,自然语言处理、数据挖掘相关,汇总最新资源,...
  • 导读/相机和激光雷达之间的时间戳同步问题一直是实时跑SLAM的先决条件。本文试图以最清晰的思路去讲明白这个事情,开始本文之前,先介绍几个基本概念。作者:郑纯然原文链接:https://z...
  • 最近在项目中需要在激光雷达(Lidar)和相机(Camera)之间进行标定,需要标定相机内参和外参,使用的是张正友标定法,这里给出其数学理论推导过程。
  • 这里要讲的激光雷达和相机的联合标定就属于空间同步范畴。 另外,现在用深度学习处理点云成为热点,在标注点云时,由于点云的稀疏性,单靠点云,很难判断目标和类别,如果有时间同步的图像,就可以判断出来,但是...
  • 激光雷达和相机的联合标定(Camera-LiDAR Calibration)之Autoware 激光雷达(lidar)和相机(camera)联合标定调研(基于Autoware的详细步骤) 无人驾驶汽车系统入门(二十二)——使用Autoware实践激光雷达与摄像机...
  • 上篇博客激光雷达和相机的联合标定(Camera-LiDAR Calibration)之Autoware介绍了为什么要做联合标定以及如何使用Autoware的标定工具。 这篇博客介绍apollo标定工具的使用方法。 首先需要安装apollo,然后才能使用...
  • 行业分类-物理装置-一种单次拍摄的LIDAR与全景相机的外参数标定方法.zip
  • 雷达相机标定(ROS)

    2020-12-22 11:27:38
    4.2.2 标定相机内参,与上述标定相似 标定单目: rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square 0.028 --size 7x6 image:=/camera/color/image_raw 标定双目: rosrun autoware_camera_...
  • 2.使用matlab标定好的相机内参矩阵 3.拥有雷达的点云文件PCD或PCL格式(实例所使用的是内置的PCD文件) 下面这幅图展示了整个标定的过程,最终两个坐标系的转换使用了刚性变换。 实现步骤 从LiDAR中加载内置点云...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 873
精华内容 349
关键字:

lidar相机标定