与相机标定类似，激光雷达也有内参（由厂家提供）和外参之分 。
文章目录
- 内参
- 外参
- 理论基础1：使用长宽已知的矩形板ABCD来标定
- 理论基础2：多线雷达使用的是纸箱法
- 1 相机与激光雷达的联合标定
- ros中的联合校准包
- MATLAB 相机与激光雷达的标定
- 输出参数含义
- 结果投影变换
- （A）联合标定发展史
- 2 激光雷达与组合惯导联合标定
- 程序1 ：（靠特征点计算）
- 程序2 ：
- 3 多激光雷达外参自动标定算法
内参
内参是内部激光发射器坐标囍和雷达自身坐标器之间的转换关系。
外参
外参是激光雷达与其他坐标系（例如车辆后轴中心）之间的坐标系转换关系。我们主要标定的是俯仰角（x轴）和侧倾角（y轴）
理论基础1：使用长宽已知的矩形板ABCD来标定
我们使用长宽已知的矩形板ABCD来标定
几何关系求解
理论基础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_z3）使用
通过如下命令进行启动：
roslaunch lidar_camera_calibration find_transform.launchMATLAB 相机与激光雷达的标定
使用工具箱：
输出参数含义
结果投影变换
通过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 激光雷达与组合惯导联合标定
首先找到一个标志物，如一个大纸箱，确定某一个角为角点（由于采用的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)];
endfunction [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 ：
按照方案一，在实际采集数据时比较困难，纸箱或车等标志物在点云中都不够明显，若要良好效果需要以一栋小平房的一角来做角点，于是提出了方案二：通过室外楼梯或台阶一条明显的点云线在地图坐标系中建立一条空间直线方程，在不同时刻，不同帧之间对应雷达坐标系中的点云线在地图坐标系中在同一条空间直线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)];
endfunction [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;
end3 多激光雷达外参自动标定算法
https://github.com/Livox-SDK/Livox_automatic_calibration
(Target-Free Automatic Calibration)并在Github上开源。该技术主要依靠几何一致性假设，即多个雷达扫描出来的局部三维模型是一致的，通过对基准雷达(LiDAR0)进行移动建图，然后将其余雷达数据对LiDAR0的重建地图不断进行迭代配准与计算，依靠一致性假设不断减少匹配误差，直到算法收敛并且满足标定矩阵刚性不变特性（六条平行线），最后用一致性算法得出最终标定矩阵（外参）。
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
