激光雷达和相机联合标定

前言

最近搞了一下激光和相机的联合标定，分别用c艹+pcl与python+open3d写了写linux/windows上相应的联合标定软件，记录一下。

绪论

方法基于标定板，目的就为了求取传感器之间的外参。
小编的代码主要做了一件事，就是选取各传感器对应的关键点，如激光中标定板的角点的3d坐标和图像中标定板角点的像素坐标。
整体流程分为以下四部分：
1. 标定板点云提取
2. 标定板点云和图像角点提取

求解外参矩阵
重投影验证标定结果
ps：open3d的api查起来比较麻烦，有很多pcl或eigen直接实现的部分，在open3d中我手鲁了一遍

1. 标定板点云提取

1.1 初筛

使用直通滤波初步限制点云范围（甚至可以根据航向角限制），通过opencv滑动条和pcl、open3d库，我们可以实时调节并显示限制范围后的点云。
之后，用统计滤波器将一些离群点去除

下面分别给一下python和c++的核心代码
python的
调用部分

    # 直通滤波create_trackbar()while 1:xyz = get_trackbar()point_cloud_copy = cloud_passthrough(point_cloud_numpy, xyz)pcd.points = o3d.utility.Vector3dVector(point_cloud_copy)vis.update_geometry(pcd) #open3d is terrible 这句话可有可无vis.poll_events()vis.update_renderer()key = cv2.waitKey(1)if key == ord('q'):vis.remove_geometry(pcd)break# 统计滤波out, _ = pcd.remove_statistical_outlier(20, 1) # parms: (在进行统计时考虑的临近点个数,判断是否为离群点的阀值)out.paint_uniform_color([1, 1, 1])vis.add_geometry(out)while 1:vis.update_geometry(out)vis.poll_events()vis.update_renderer()key = cv2.waitKey(1)if key == ord('q'):vis.remove_geometry(out)break

函数实现

def cloud_passthrough(cloud_np, xyz):x_min = xyz[0]x_max = xyz[1]y_min = xyz[2]y_max = xyz[3]z_min = xyz[4]z_max = xyz[5]cloud_np = cloud_np[cloud_np[:, 0] < x_max] cloud_np = cloud_np[cloud_np[:, 0] > x_min]                    cloud_np = cloud_np[cloud_np[:, 1] < y_max]cloud_np = cloud_np[cloud_np[:, 1] > y_min]cloud_np = cloud_np[cloud_np[:, 2] < z_max]cloud_np = cloud_np[cloud_np[:, 2] > z_min]return cloud_np

c++的

while(!viewer1->wasStopped()){// 判断标定板点云是否直通滤波完毕if(chose_bd) {chose_bd = 0;break;}// 每次都对原始点云进行直通滤波操作pcl::copyPointCloud(*cloud, *cloud_show);// 更新直通滤波参数xyzMutex.lock();x0 = x_min;x1 = x_max;y0 = y_min;y1 = y_max;z0 = z_min;z1 = z_max;xyzMutex.unlock();// 进行直通滤波cloudPassThrough(cloud_show, "y", y0, y1);cloudPassThrough(cloud_show, "x", x0, x1);cloudPassThrough(cloud_show, "z", z0, z1);// 更新可视化viewerviewer1->removePointCloud("test");pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_show, "z");viewer1->addPointCloud(cloud_show,fildColor, "test");boost::this_thread::sleep(boost::posix_time::microseconds(1000));viewer1->spinOnce(100);}// 统计滤波 去除离群点cloudStatisticalOutlierRemoval(cloud_show);

1.2 拟合平面并投影

初筛完标定板点云后，仍有离群点和波动存在，我们通过拟合平面，并且将原始点云投影于该平面的方法，让标定板点云在空间处于统一平面。
python的
调用部分

    # 拟合平面inlier_cloud = copy.copy(out)plane_model, inliers = get_plane(inlier_cloud)[a, b, c, d] = plane_modelprint(f"Plane equation:{a:.2f}x +{b:.2f}y +{c:.2f}z +{d:.2f}= 0")inlier_cloud = inlier_cloud.select_by_index(inliers)inlier_cloud.paint_uniform_color([0, 1, 0])vis.add_geometry(inlier_cloud)while 1:vis.update_geometry(inlier_cloud)vis.poll_events()vis.update_renderer()key = cv2.waitKey(1)if key == ord('q'):vis.remove_geometry(inlier_cloud)break# 投影滤波project_cloud = copy.copy(inlier_cloud)project_cloud.points = project_filter(project_cloud, plane_model)vis.add_geometry(project_cloud)while 1:vis.update_geometry(project_cloud)vis.poll_events()vis.update_renderer()key = cv2.waitKey(1)if key == ord('q'):#vis.remove_geometry(inlier_cloud)break

函数实现部分

def get_plane(pcd):# parms:#           distance_threshold#           ransac_n#           num_iterationsplane_model, inliers = pcd.segment_plane(0.09, 10, 1000)# dis应该在0.04-0.05，但是标定板中间凹陷return plane_model, inliersdef project_filter(cloud, plane_model):cloud = np.asarray(cloud.points)[a, b, c, d] = plane_modeldis = a*a+b*b+c*cx = copy.deepcopy(cloud[:, 0])y = copy.deepcopy(cloud[:, 1])z = copy.deepcopy(cloud[:, 2])cloud[:, 0] = ((b*b+c*c)*x - a*(b*y + c*z + d))/discloud[:, 1] = ((a*a+c*c)*y - b*(a*x + c*z + d))/discloud[:, 2] = ((b*b+a*a)*z - c*(a*x + b*z + d))/disreturn o3d.utility.Vector3dVector(cloud)

c++de

  //-------------------------- 模型估计 --------------------------cout << "->正在估计平面..." << endl;pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud_show));   //选择拟合点云与几何模型pcl::RandomSampleConsensus<PointT> ransac(model_plane);  //创建随机采样一致性对象ransac.setDistanceThreshold(0.1); //设置距离阈值，与平面距离小于0.01的点作为内点ransac.computeModel();              //执行模型估计PointCloudT::Ptr cloud_plane(new PointCloudT);   //---------- 根据索引提取内点 ----------// 方法1vector<int> inliers;                //存储内点索引的向量ransac.getInliers(inliers);           //提取内点对应的索引pcl::copyPointCloud<PointT>(*cloud_show, inliers, *cloud_plane);// 输出模型参数Ax+By+Cz+D=0Eigen::VectorXf coefficient;ransac.getModelCoefficients(coefficient);cout << "平面方程为：\n"<< coefficient[0] << "x + "<< coefficient[1] << "y + "<< coefficient[2] << "z + "<< coefficient[3] << " = 0"<< endl;//----------------------- 可视化拟合结果 -----------------------viewer1->addPointCloud<pcl::PointXYZ>(cloud_show, "cloud");                                             //添加原始点云viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");   //颜色viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");  //点的大小viewer1->addPointCloud<pcl::PointXYZ>(cloud_plane, "plane");                                          //添加平面点云viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane");   //颜色viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");  //点的大// 创建投影器pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());// 由ax+by+cz+d=0设置投影面参数coefficients->values.resize(4);coefficients->values[0] = coefficient[0];coefficients->values[1] = coefficient[1];coefficients->values[2] = coefficient[2];coefficients->values[3] = coefficient[3];// 创建投影滤波对象pcl::ProjectInliers<pcl::PointXYZ>proj;// 设置对象对应的投影模型proj.setModelType(pcl::SACMODEL_PLANE);// 设置输入点云proj.setInputCloud(cloud_plane);// 设置模型对应的系数proj.setModelCoefficients(coefficients);// 执行投影滤波PointCloudT::Ptr cloud_filtered(new PointCloudT);    proj.filter(*cloud_filtered);

做到这一步，我们搞个效果视频看一下，不然太抽象了

calibration

2. 标定板点云和图像角点提取

我们想通过3d to 2d，来简化点云聚类、拟合、最终寻找到角点的过程。

2.1 点云降维

我们先将投影得到同一平面上的点云进行平移、旋转，将其移动至xoy面，
经此操作，3d点云z坐标均为0，我们可以对点云进行图像层面操作。

c++的

    // 将标定板点云旋转平移至xoy面PointCloudT::Ptr cloud_hhh(new PointCloudT);PointCloudT::Ptr cloud_rotate(new PointCloudT);while(!viewer1->wasStopped()){// 判断投影是否完成if(chose_bd){viewer1->removeAllPointClouds();PointCloudT::Ptr cloud_hhh(new PointCloudT);PointCloudT::Ptr cloud_rotate(new PointCloudT);// 计算过原点平面法线方程float A = coefficient[0];float B = coefficient[1];float C = coefficient[2];float D = coefficient[3];float Z0 = -D/C;float x0 = cloud_filtered->points[0].x;float y0 = cloud_filtered->points[0].y;float z0 = cloud_filtered->points[0].z;// 将点云平面平移至原点所在平面for (int i=0; i<cloud_filtered->points.size(); i++){float x = cloud_filtered->points[i].x-x0;float y = cloud_filtered->points[i].y-y0;float z = cloud_filtered->points[i].z-z0;PointT t0 = {x, y, z};cloud_hhh->points.push_back(t0);}// 法线可视化PointT a,b;a.x = -10*A;a.y = -10*B;a.z = -10*C;b.x = 10*A;b.y = 10*B;b.z = 10*C;viewer1->addLine<pcl::PointXYZ>(a, b, 255, 255, 255, "line1"); PointCloudT::Ptr linePoint(new PointCloudT);linePoint->points.push_back(a);linePoint->points.push_back(b);// 将点云绕Y轴旋转Eigen::Affine3f transform_2=Eigen::Affine3f::Identity();float theta = - atan(A/C);cout << theta*180/M_PI << endl;transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitY()));printf ("\n Method #2: using an Affine3f \n");std::cout << transform_2.matrix() << std::endl;pcl::transformPointCloud(*cloud_hhh, *cloud_rotate, transform_2);// 绘制旋转后平面法线pcl::transformPointCloud(*linePoint, *linePoint, transform_2);a = linePoint->points[0];b = linePoint->points[1];viewer1->addLine<pcl::PointXYZ>(a, b, 255, 0, 0, "line2"); // 将点云绕X轴旋转float new_B = b.y/10;float new_C = b.z/10;Eigen::Affine3f transform_1=Eigen::Affine3f::Identity();float theta1 = atan(new_B/new_C);cout << theta1*180/M_PI << endl;transform_1.rotate(Eigen::AngleAxisf(theta1, Eigen::Vector3f::UnitX()));std::cout << transform_1.matrix() << std::endl;pcl::transformPointCloud(*cloud_rotate, *cloud_rotate, transform_1);// 绘制旋转后平面法线pcl::transformPointCloud(*linePoint, *linePoint, transform_1);a = linePoint->points[0];b = linePoint->points[1];viewer1->addLine<pcl::PointXYZ>(a, b, 0, 255, 0, "line3");

python的
调用部分

    # 平移旋转至xoy面trans_cloud = copy.copy(project_cloud)trans_cloud.points, euler, xyz0 = transform(trans_cloud, plane_model)vis.add_geometry(trans_cloud)while 1:vis.update_geometry(trans_cloud)vis.poll_events()vis.update_renderer()key = cv2.waitKey(1)if key == ord('q'):vis.remove_geometry(trans_cloud)break

函数实现部分

def transform(cloud, plane_model):cloud = np.asarray(cloud.points)[a, b, c, d] = plane_model# same# R = o3d.geometry.get_rotation_matrix_from_axis_angle(np.array([pitch,0,0],dtype='float64'))yaw = atan(a/c)Ry = np.array([[cos(yaw), 0, sin(yaw)],[0, 1, 0],[-sin(yaw), 0, cos(yaw)]], dtype='float64')Euler = np.array([a, b, c], dtype='float64')a, b, c = np.dot(Euler, Ry)pitch = -atan(b/c)Rx = np.array([[1, 0, 0],[0, cos(pitch), -sin(pitch)],[0, sin(pitch), cos(pitch)]], dtype='float64')R = np.dot(Ry, Rx)cloud = np.dot(cloud, R)x0 = cloud[0][0]y0 = cloud[0][1]z0 = cloud[0][2]# 平移cloud[:, 0] -= x0cloud[:, 1] -= y0cloud[:, 2] -= z0return o3d.utility.Vector3dVector(cloud), [-pitch, -yaw], np.array([x0,y0,z0],dtype='float64')

2.2 聚类求取角点

这里采用了dbscan的聚类算法，点
之间距离的计算是以x方向的欧氏
距离。
python实现

import numpy as np
import random
import matplotlib.pyplot as plt
import mathdis_draw = []#计算两点之间的欧式距离
def calDist(p1 , p2):sum = 0for x1 , x2 in zip(p1 , p2):sum += (x1 - x2) ** 2return sum ** 0.5
# 计算X方向距离
def calDistX(p1, p2):return abs(p1[0] - p2[0])# 点云结构体
class CloudPoint():def __init__(self, xyz):self.visited = 0 # 是否遍历过self.coreobj = 0 # 是否是核心点self.clusterid = -1 # 类别self.neibor = [] # 邻域索引self.xyz = xyz # 坐标def setVisited(self):self.visited = 1def setClusterid(self, id):self.clusterid = iddef setCoreobj(self):self.coreobj = 1def getNeibor(self):return self.neibor#获取一个点的ε-邻域（记录的是索引）
def getNeibor(dataSet , e, minPts):for i in range(len(dataSet)):for j in range(len(dataSet)):if i != j:dis_draw.append(calDistX(dataSet[j].xyz, dataSet[i].xyz))if calDistX(dataSet[j].xyz, dataSet[i].xyz) < e:dataSet[i].neibor.append(j)if len(dataSet[i].getNeibor()) > minPts: # 设置核心点dataSet[i].setCoreobj()# 在点的邻域内聚类
def keyPointCluster(dataSet, id, clusterid):if dataSet[id].coreobj == 0: returnfor j in dataSet[id].neibor: # 遍历对象邻域域内点ID列表if dataSet[j].visited == 0: # 未遍历过则加入当前的类中dataSet[j].setClusterid(clusterid)dataSet[j].visited = 1if dataSet[j].coreobj == 1: keyPointCluster(dataSet, j, clusterid) # 递归地对该领域点数据的领域内的点执行聚类操作# 密度聚类算法
def DBSCAN(dataSet , e , minPts):C = []n = len(dataSet)# 获取点的邻域、核心点getNeibor(dataSet, e, minPts)clusterid = 0  # 聚类id计数，初始化为0for i in range(n):if dataSet[i].visited == 0 and dataSet[i].coreobj == 1: # 未遍历且是核心点dataSet[i].setClusterid(clusterid) # 设置该对象所属类ID为clusteriddataSet[i].setVisited() # 设置该对象已被访问过keyPointCluster(dataSet, i, clusterid) # 对该对象邻域内点进行聚类clusterid += 1# 结果存入lists = set()for i in range(n):if dataSet[i].clusterid != -1:if dataSet[i].clusterid not in s: # 类别id没有在s中则新建一个listC.append([dataSet[i].xyz])s.add(dataSet[i].clusterid)else:C[dataSet[i].clusterid].append(dataSet[i].xyz)return Cdef draw(c, clouds):for i in list(range(len(c))):index = c[i]x, y = [], []for j in index:           x.append(j[0])y.append(j[1])plt.scatter(x, y)plt.show()# 计算轮廓点
def getBoundpoint(c, clouds):bPointList = []for i in list(range(len(c))):cloud = c[i]cenp = [np.mean([i[0] for i in cloud]), np.mean([i[1] for i in cloud]), np.mean([i[2] for i in cloud])] # 类中心点disList = [calDist(cenp, i) for i in cloud] # 计算到中心点的距离bPoint1 = cloud[disList.index(max(disList))]disList = [calDist(bPoint1, i) for i in cloud] # 计算到一个轮廓点的距离bPoint2 = cloud[disList.index(max(disList))]if bPoint1[1] > bPoint2[1]: # 从上到下排序bPointList.append(([bPoint1, bPoint2], cenp[0]))else:bPointList.append(([bPoint2, bPoint1], cenp[0]))bPointList.sort(key=lambda x:x[1]) # 从左到右排序return [x[0] for x in bPointList]# 轮廓点分类
def bPointSep(bPointList):result = []ulist = [i[0][1] for i in bPointList]llist = [i[1][1] for i in bPointList]uMax = ulist.index(max(ulist))lMin = llist.index(min(llist))result.append([bPointList[i][0] for i in range(len(ulist)) if i <= uMax])result.append([bPointList[i][1] for i in range(len(ulist)) if i <= lMin])result.append([bPointList[i][0] for i in range(len(ulist)) if i >= uMax])result.append([bPointList[i][1] for i in range(len(ulist)) if i >= lMin])return result        # 直线拟合
def linefit(x, y):N = float(len(x))sx, sy, sxx, syy, sxy=0, 0, 0, 0, 0for i in range(0,int(N)):sx  += x[i]sy  += y[i]sxx += x[i] * x[i]syy += y[i] * y[i]sxy += x[i] * y[i]a = (sy * sx / N - sxy) / (sx * sx / N - sxx)b = (sy - a * sx) / Nreturn a, bdef findCornerPoint(clouds):# 点云聚类C = DBSCAN(clouds, 0.12, 1)# 寻找轮廓点bPointList = getBoundpoint(C, clouds)# 轮廓点分类result = bPointSep(bPointList)# 拟合直线lines = []for i in list(len(result)):x, y = [], []for j in range(len(result[i])):           x.append(result[i][j][0])y.append(result[i][j][1])lines.append(linefit(x, y))# 计算角点cornerPoint = []for i in [[0, 2], [2, 3], [1, 3], [0, 1]]:a, b = ix = (lines[a][1] - lines[b][1]) / (lines[b][0] - lines[a][0])y = lines[a][0] * x + lines[a][1]cornerPoint.append([x, y])return cornerPoint

c++实现

/* 获取标定板算法由python程序改写，逻辑较难理解，请参考python提取标定板程序 */
void keyPointCluster(vector<MyPointCloud>& my_clouds, int id, int clusterid)
{// 获取分类关键点if(my_clouds[id].coreobj != 0){for (int k=0; k<my_clouds[id].useNum+1; k++){int j = my_clouds[id].neibor[k];if(my_clouds[j].visited == 0){my_clouds[j].clusterid = clusterid;my_clouds[j].visited = 1;if(my_clouds[j].coreobj == 1)keyPointCluster(my_clouds, j, clusterid);}}}else return ;
}
vector<vector<vector<float> > > DBSCAN(vector<MyPointCloud>& my_clouds, float e, float minPts)
{// 密度聚类算法int n = my_clouds.size();// get neiborfor (int i=0; i<n; i++){for (int j=0; j<n; j++){if (i!=j){float x0 = my_clouds[i].x;float y0 = my_clouds[i].y;float x1 = my_clouds[j].x;float y1 = my_clouds[j].y;float dis = pow(pow(x0-x1,2),0.5);int index = 0;if (dis < e){while(my_clouds[i].neibor[index] != -1){index++;}my_clouds[i].neibor[index] = j;my_clouds[i].useNum = index;}}if(my_clouds[i].useNum+1 > minPts)my_clouds[i].coreobj = 1;}}int clusterid = 0;for (int i=0; i<n; i++){if(my_clouds[i].visited == 0 && my_clouds[i].coreobj == 1){my_clouds[i].clusterid = clusterid;my_clouds[i].visited = 1;keyPointCluster(my_clouds, i, clusterid);clusterid ++;}}vector<vector<vector<float> > > C_out;// 结果存入C_outfor(int j=0; j<clusterid+1; j++){vector<vector<float> > points;for(int i=0; i<n; i++){if(my_clouds[i].clusterid == j){vector<float> point;point.push_back(my_clouds[i].x);point.push_back(my_clouds[i].y);point.push_back(my_clouds[i].z);points.push_back(point);}}C_out.push_back(points);}return C_out;
}
vector<vector<vector<float> > > getBoundPoint(vector<vector<vector<float> > >& c)
{// 计算轮廓点并进行边属分类vector<vector<vector<float> > > bPointList;vector<float> meanX;for(int i=0; i<c.size()-1; i++){float x_mean=0; // 万万需要初始化float y_mean=0;for(int j=0; j<c[i].size(); j++){x_mean += c[i][j][0];y_mean += c[i][j][1];}x_mean /= c[i].size();y_mean /= c[i].size();meanX.push_back(x_mean);float maxDistoCenter = -9;int index1 = 0;for(int j=0; j<c[i].size(); j++){float dis = pow(pow(x_mean-c[i][j][0],2)+pow(y_mean-c[i][j][1],2),0.5);if (dis > maxDistoCenter){index1 = j;maxDistoCenter = dis;}}vector<vector<float> > pts;vector<float> pt0;    pt0.push_back(c[i][index1][0]);pt0.push_back(c[i][index1][1]);float maxDistoEdge = -9;int index2=0;for(int j=0; j<c[i].size(); j++){float dis = pow(pow(c[i][index1][0]-c[i][j][0],2)+pow(c[i][index1][1]-c[i][j][1],2),0.5);if (dis > maxDistoEdge){index2 = j;maxDistoEdge = dis;}}vector<float> pt1;pt1.push_back(c[i][index2][0]);pt1.push_back(c[i][index2][1]);if(pt0[1] > pt1[1]){pts.push_back(pt0);pts.push_back(pt1);}else{pts.push_back(pt1);pts.push_back(pt0);}bPointList.push_back(pts);}/*for(int i=0; i<meanX.size();i++)cout << meanX[i] << " ";cout << endl;*/// bubblesort 尽量迭代，就不快排了bool sorted = false;while(!sorted){sorted = true;for(int j=0; j<meanX.size()-1; j++){if(meanX[j] > meanX[j+1]){vector<vector<float> > temp;temp = bPointList[j];bPointList[j] = bPointList[j+1];bPointList[j+1] = temp;float tmp = meanX[j+1];meanX[j+1] = meanX[j];meanX[j] = tmp;sorted = false;}}}// 轮廓点分类float uMax = -9;float uMin = 9;int uMax_index = 0;int uMin_index = 0;for (int i=0; i<bPointList.size(); i++){if(bPointList[i][0][1] > uMax){uMax = bPointList[i][0][1];uMax_index = i;}if(bPointList[i][1][1] < uMin){uMin = bPointList[i][1][1];uMin_index = i;}}vector<vector<vector<float> > > PointSep;int length = bPointList.size();vector<vector<float> > l0;vector<vector<float> > l1;vector<vector<float> > l2;vector<vector<float> > l3;for (int i=0; i<length; i++){if (i <= uMax_index){l0.push_back(bPointList[i][0]);}if (i <= uMin_index){l1.push_back(bPointList[i][1]);}if (i >= uMax_index){l2.push_back(bPointList[i][0]);}if (i >= uMin_index){l3.push_back(bPointList[i][1]);}}PointSep.push_back(l0);PointSep.push_back(l1);PointSep.push_back(l2);PointSep.push_back(l3);for (int i=0; i<PointSep.size(); i++){for (int j=0; j<PointSep[i].size(); j++){cout << "[ ";for (int k=0; k<2; k++){cout << PointSep[i][j][k] << " ";}cout << " ]";}cout << endl;}return PointSep;
}
vector<vector<float> > findCornerPoint(vector<vector<vector<float> > >& PointSep)
{// 获取角点vector<vector<float> > result;vector<vector<float> > line_kb;for(int i=0; i<PointSep.size(); i++){vector<float> x;vector<float> y;for(int j=0; j<PointSep[i].size(); j++){x.push_back(PointSep[i][j][0]);y.push_back(PointSep[i][j][1]);}// 直线拟合float N = (float)PointSep[i].size();float sx = 0;float sy = 0;float sxx = 0;float syy = 0;float sxy = 0;for(int j=0; j<(int)N; j++){sx += x[j];sy += y[j];sxx += x[j]*x[j];syy += y[j]*y[j];sxy += x[j]*y[j];}float a = (sy * sx / N - sxy) / (sx * sx / N - sxx);float b = (sy - a * sx) / N;vector<float> kb = {a, b};line_kb.push_back(kb);}int a0,b0;float xc,yc;vector<float> xy;a0 = 0;b0 = 2;xc = (line_kb[a0][1] - line_kb[b0][1]) / (line_kb[b0][0] - line_kb[a0][0]);yc = line_kb[a0][0] * xc + line_kb[a0][1];xy = {xc, yc};result.push_back(xy);a0 = 2;b0 = 3;xc = (line_kb[a0][1] - line_kb[b0][1]) / (line_kb[b0][0] - line_kb[a0][0]);yc = line_kb[a0][0] * xc + line_kb[a0][1];xy = {xc, yc};result.push_back(xy);a0 = 1;b0 = 3;xc = (line_kb[a0][1] - line_kb[b0][1]) / (line_kb[b0][0] - line_kb[a0][0]);yc = line_kb[a0][0] * xc + line_kb[a0][1];xy = {xc, yc};result.push_back(xy);a0 = 0;b0 = 1;xc = (line_kb[a0][1] - line_kb[b0][1]) / (line_kb[b0][0] - line_kb[a0][0]);yc = line_kb[a0][0] * xc + line_kb[a0][1];xy = {xc, yc};result.push_back(xy);return result;
}

求取角点的部分也附上了，看下效果

2.3 结果

参考点云降维的部分，我们将2d转回3d点云原位置即可得到结果
视觉手动选点，采用opencv回调函数，这些略

3.PNP解算和重投影

这部分也就给两张图，略

引用参考的东西蛮多蛮碎的，如未指出，请联系。
本人邮箱sumanghhx@163.com，亦欢迎建议。