与相机标定类似，激光雷达也有内参（由厂家提供）和外参之分 。
文章目录
内参
内参是内部激光发射器坐标囍和雷达自身坐标器之间的转换关系。
外参
外参是激光雷达与其他坐标系（例如车辆后轴中心）之间的坐标系转换关系。我们主要标定的是俯仰角（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的重建地图不断进行迭代配准与计算，依靠一致性假设不断减少匹配误差，直到算法收敛并且满足标定矩阵刚性不变特性（六条平行线），最后用一致性算法得出最终标定矩阵（外参）。
扫一扫
178
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
