结构光测距相位差_利用线结构光进行三维重构（测距）
利用线结构光进行三维重构(测距)
通过线激光器扫描物体，同时用摄像机对其拍照得到带有结构光的图片，提取结构光上的点的三维坐标，激光器扫描整个物体就可求出所有点的三维坐标实现物体表面的三维重构，即可测量物体表面任意两点距离。
准备知识：
四个坐标系的转换
世界坐标系--摄像机坐标系
将摄像机光心定位摄像机坐标原点Oc，摄像机的光轴定位摄像机坐标系的Zc轴，Xc轴，Yc轴分别与图像坐标系的x轴y轴平行。
R为3阶正交单位矩阵，t为平移向量，均为相机外参数
物理坐标系--像素坐标系
图像的x，y轴分别和像素的u，v轴平行
u=x/dx+u0, v=y/dy+v0
物理坐标系--摄像机坐标系
像素坐标系--世界坐标系
fx，fy，u0，v0为摄像机内部参数，R，t，为外部参数
系统测量模型
P点既在OP’直线上又在光平面上(由结构光投射器与物体表面结构光构成的平面)，摄像机和结构光投射器相对位置不变，光平面方程不变，设摄像机下光平面方程为
设点P’在摄像机坐标系下的图像坐标(x，y，1)P点坐标为(Xc，Yc，Zc)则直线OcP直线方程为
联立可得
求空间点在摄像机坐标系下的空间三维坐标需要光平面方程，P’点的图像坐标，求解P’的坐标需要摄像机内参数，所以需要摄像机标定和光平面标定。
系统实现方案
固定摄像机和激光器，移动器棋盘格拍摄带有结构光和不带结构光的图片，需要不同角度拍摄。
通过harris角点提取图片中角点像素坐标
运用张正友标定思想完成相机标定
提取线结构光方程，与图像角点直线方程结合，求角点直线和结构光的交点坐标
通过角点的像素坐标和角点与结构光的交点坐标运用交比不变性得到结构光上一系列交点的摄像机坐标，通过最小二乘拟合得到摄像机坐标系下的光平面方程
运用光平面方程，可得到结构光上任意一点的相机坐标
相机标定和光平面实现
Qt+openCV
cameraCalibrate函数原型
calibrateCamera(InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints,
Size imageSize,
InputOutputArray cameraMatrix,
InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs,
OutputArrayOfArrays tvecs,
int flags=0 );
参数 ：
objectPoints 初始化世界坐标系的所有角点的三维坐标点
应输入 vector> objectPoints
imagePoints 与其对应的像素坐标系的所有角点的二维坐标点
应输入 vector< vector< Point2f>> imagePoints
imageSize 图像大小
cameraMatrix 相机内参数矩阵
输入一个cv::Mat cameraMatrix即可。
distCoeffs 为畸变矩阵。输入一个cv::Mat distCoeffs即可
rvecs 为旋转向量 应输入一个vector<:mat>
tvecs 为平移向量 应输入一个vector<:mat>
flags为标定是所采用的算法。可如下某个或者某几个参数：
CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,cx,cy的估计值。否则的话，将初始化(cx,cy)图像的中心点，使用最小二乘估算出fx，fy。如果内参数矩阵和畸变居中已知的时候，应该标定模块中的solvePnP()函数计算外参数矩阵。
CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值。
CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。
CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数(p1,p2)为零。
CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。如果设置了CV_CALIB_USE_INTRINSIC_GUESS参数，
CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。
摄像机标定需要角点的世界坐标棋盘格表面为x-y轴，所以棋盘格角点z坐标全为0，已知两角点的实际距离就可求出所有角点的世界坐标
摄像机标定还需角点的像素坐标
利用openCV库函数findChessboardCorners()可以提取到。
带入这两个参数到cameraCalibrate函数即可求出相机内参数和每幅图对应的外参数
光平面标定
1、提取线结构光中心线方程，结构光处像素值为255利用二值化原理提取只含结构光的图像，利用goodFeaturesToTrack()取得结构光上的点，将这些点代入fitline()，拟合得到线结构光方程。
2、拟合角点直线方程，结合线结构光中心线方程，求两直线交点可得一系列光条上的点的像素坐标。
3、通过角点世界坐标，结合相机所求每一幅图的相机外参数，求出每幅图的角点摄像机坐标
4、通过摄像机内参数，将角点图像坐标转换为角点物理坐标
5、已知A B C 角点D为角点直线与结构光角点直线的交点，A’ B ‘ C’ D’为成像点,
已知A’ B ‘ C’ D’四点物理坐标或像素坐标，A B C三点相机坐标可求出D的相机坐标。
6、对于多幅图求出物体表面结构光与角点直线的交点坐标运用最小二乘拟合得到光平面方程
运用光平面方程求距离
鼠标点击结构光上任意两点获取两点的像素坐标通过公式
其中1/dx=fx，1/dy=fy，fx，fy均为摄像机内参数
然后物理坐标转为摄像机坐标
运用两点间距离公式可求出结构光上两点实际距离。
附：
最小二乘拟合平面
CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);
//定义用来存储需要拟合点的矩阵大小N*3；
for (int i=0;i
{
points_mat->data.fl[i*3+0] = X_vector[i];
//矩阵的值进行初始化 X的坐标值
points_mat->data.fl[i * 3 + 1] = Y_vector[i];
// Y的坐标值
points_mat->data.fl[i * 3 + 2] = Z_vector[i];
//
// Z的坐标值
}
float plane12[4] = { 0 };//定义用来储存平面参数的数组
cvFitPlane(points_mat, plane12);//调用方程
// 其中 Plane12[4] 数组中对应ABCD；
//Ax+by+cz=D
void cvFitPlane(const CvMat* points, float* plane){
// Estimate geometric centroid.
int nrows = points->rows;
int ncols = points->cols;
int type = points->type;
CvMat* centroid = cvCreateMat(1, ncols, type);
cvSet(centroid, cvScalar(0));
for (int c = 0; c
for (int r = 0; r
{
centroid->data.fl[c] += points->data.fl[ncols*r + c];
}
centroid->data.fl[c] /= nrows;
}
// Subtract geometric centroid from each point.
CvMat* points2 = cvCreateMat(nrows, ncols, type);
for (int r = 0; r
for (int c = 0; c
points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
// Evaluate SVD of covariance matrix.
Cv