此处重点讨论在特定条件下，重建场景的三维结构和相机的三维姿态的一些应用实现。下面是完整投影公式最通用的表示方式。

        在上述公式中，可以了解到，真实物体转为平面之后，s系数丢失了，因而无法会的三维坐标，而s系数其实是与直接距离Z相关，实际上，如果知道物体到相机成像平面距离，相机的内参、外参就可以获得空间坐标：

1. 相机标定

       相机标定就是设置相机各种参数（即投影公式中的内参，外参）的过程。当然也可以使用相机厂家提供的技术参数，利用正确的相机标定方法，即可得到精确的标定信息。

      相机标定的基本原理：确定场景中一系列点的三维坐标并拍摄这个场景，然后观测这些点
在图像上投影的位置。有了足够多的三维点和图像上对应的二维点，就可以根据投影方程推断出
准确的相机参数。

      （1）一种方法是对一个包含大量三维点的场景取像（一些车辆在出场会使用）。

      （2）更实用的做法是从不同的视角为一些三维点拍摄多个照片。这种方法相对比较简单，但是它除了需要计算相机本身的参数，还需要计算每个相机视图的位置，OpenCV 推荐使用国际象棋棋盘的图案生成用于标定的三维场景点的集合。

        这个图案在每个方块的角点位置创建场景点；由于图案是平面的，可以假设棋盘位于Z=0 且X 和Y 的坐标轴与网格对齐的位置。

// 1. 输出图像角点的向量
std::vector<cv::Point2f> imageCorners;
// 棋盘内部角点的数量
cv::Size boardSize(7,5);
// 获得棋盘角点
bool found = cv::findChessboardCorners(
image, // 包含棋盘图案的图像
boardSize, // 图案的尺寸
imageCorners); // 检测到的角点列表// 2. 画出角点
cv::drawChessboardCorners(image, boardSize,
imageCorners, found); // 找到的角点

        连接角点的线条的次序，就是角点在向量中存储的次序。在进行标定前，需要指定相关的三维点。指定这些点时可自由选择单位（例如厘米或英寸），不过最简单的办法是将方块的边长指定为一个单位。这样第一个点的坐标就是(0, 0, 0)（假设棋盘的纵深坐标为Z = 0），第二个点的坐标是(1, 0, 0)，最后一个点的坐标是(6, 4, 0)。这个图案共有35 个点；若要进行精确的标定，这些点是远远不够的。为了得到更多的点，需要从不同的视角对同一个标定图案拍摄更多的照片。可以在相机前移动图案，也可以在棋盘周围移动相机。从数学的角度看，这两种方法是完全等效的。OpenCV 的标定函数假定由标定图案确定坐标系，并计算相机相对于坐标系的旋转量和平移量。

// 打开棋盘图像，提取角点
int CameraCalibrator::addChessboardPoints(
const std::vector<std::string> & filelist, // 文件名列表
cv::Size & boardSize) { // 标定面板的大小
// 棋盘上的角点
std::vector<cv::Point2f> imageCorners;
std::vector<cv::Point3f> objectCorners;
// 场景中的三维点：
// 在棋盘坐标系中，初始化棋盘中的角点
// 角点的三维坐标(X,Y,Z)= (i,j,0)
for (int i=0; i<boardSize.height; i++) {
for (int j=0; j<boardSize.width; j++) {
objectCorners.push_back(cv::Point3f(i, j, 0.0f));
}
}
// 图像上的二维点：
cv::Mat image; // 用于存储棋盘图像
int successes = 0;
// 处理所有视角
for (int i=0; i<filelist.size(); i++) {
// 打开图像
image = cv::imread(filelist[i],0);
// 取得棋盘中的角点
bool found = cv::findChessboardCorners(
image, // 包含棋盘图案的图像
boardSize, // 图案的大小
imageCorners); // 检测到角点的列表
// 取得角点上的亚像素级精度
if (found) {
cv::cornerSubPix(image, imageCorners,
cv::Size(5,5), // 搜索窗口的半径
cv::Size(-1,-1),
cv::TermCriteria( cv::TermCriteria::MAX_ITER +
cv::TermCriteria::EPS,30, // 最大迭代次数
0.1)); // 最小精度
// 如果棋盘是完好的，就把它加入结果
if (imageCorners.size() == boardSize.area()) {
// 加入从同一个视角得到的图像和场景点
addPoints(imageCorners, objectCorners);
successes++;
}
}
// 如果棋盘是完好的，就把它加入结果
if (imageCorners.size() == boardSize.area()) {
// 加入从同一个视角得到的图像和场景点
addPoints(imageCorners, objectCorners);
successes++;
}
}
return successes;
}

             

    处理完足够数量的棋盘图像后（这时就有了大量的三维场景点/二维图像点的对应关系），就可以开始计算标定参数了：

// 返回重投影误差
double CameraCalibrator::calibrate(cv::Size &imageSize) {
// 输出旋转量和平移量
std::vector<cv::Mat> rvecs, tvecs;
// 开始标定
return calibrateCamera(objectPoints, // 三维点imagePoints, // 图像点imageSize, // 图像尺寸cameraMatrix, // 输出相机矩阵distCoeffs, // 输出畸变矩阵rvecs, tvecs, // Rs、Tsflag); // 设置选项
}
//根据经验，10~20 个棋盘图像就足够了，但是这些图像的深度和拍摄视角必须不同

       用刚标定的相机拍摄的所有图像，在标定类中增加了一个额外畸变矫正的方法：

// 去除图像中的畸变（标定后）
cv::Mat CameraCalibrator::remap(const cv::Mat &image) {
cv::Mat undistorted;
if (mustInitUndistort) { // 每个标定过程调用一次
cv::initUndistortRectifyMap(
cameraMatrix, // 计算得到的相机矩阵
distCoeffs, // 计算得到的畸变矩阵
cv::Mat(), // 可选矫正项（无）
cv::Mat(), // 生成无畸变的相机矩阵
image.size(), // 无畸变图像的尺寸
CV_32FC1, // 输出图片的类型
map1, map2); // x 和y 映射功能
mustInitUndistort= false;
}
// 应用映射功能
cv::remap(image, undistorted, map1, map2,
cv::INTER_LINEAR); // 插值类型
return undistorted;
}

2. 相机姿态还原

       标定后，相机就可以用来构建照片与现实场景的对应关系。如果一个物体的三维结构是已知的，就能得到它在相机传感器上的成像情况。如果该方程中的大多数项目是已知的，利用若干张照片，就可以计算出其他元素（二维或三维）的值。在已知三维结构的情况下，计算出相机的姿态。

// 根据三维/二维点得到相机姿态
cv::Mat rvec, tvec;
cv::solvePnP(
objectPoints, imagePoints, // 对应的三维/二维点
cameraMatrix, cameraDistCoeffs, // 标定
rvec, tvec); // 输出姿态
// 转换成三维旋转矩阵
cv::Mat rotation;
cv::Rodrigues(rvec, rotation);

     本质是求解刚体变换（旋转和平移），这就是透视n 点定位（Perspective-n-Point，PnP）问题，把物体坐标转换到以相机为中心的坐标系上（即以焦点为坐标原点）。

     在OpenCV 中，cv::viz 是一个基于可视化工具包（Visualization Toolkit，VTK）的附加模块。它是一个强大的三维计算机视觉框架，可以创建虚拟的三维环境，并添加各种物体。它会创建可视化的窗口，用来显示从特定视角观察到的虚拟环境。

// 1  创建viz 窗口
cv::viz::Viz3d visualizer("Viz window");
visualizer.setBackgroundColor(cv::viz::Color::white());// 2 创建一个虚拟相机
cv::viz::WCameraPosition cam(
cMatrix, // 内部参数矩阵
image, // 平面上显示的图像
30.0, // 缩放因子
cv::viz::Color::black());
// 在环境中添加虚拟相机
visualizer.showWidget("Camera", cam);// 3 用长方体表示虚拟的长椅
cv::viz::WCube plane1(cv::Point3f(0.0, 45.0, 0.0),
cv::Point3f(242.5, 21.0, -9.0),
true, // 显示线条框架
cv::viz::Color::blue());
plane1.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
cv::viz::WCube plane2(cv::Point3f(0.0, 9.0, -9.0),
cv::Point3f(242.5, 0.0, 44.5),
true, // 显示线条框架
cv::viz::Color::blue());
plane2.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);// 4 把虚拟物体加入到环境中
visualizer.showWidget("top", plane1);
visualizer.showWidget("bottom", plane2);cv::Mat rotation;
// 将rotation 转换成3×3 的旋转矩阵
cv::Rodrigues(rvec, rotation);
// 移动长椅
cv::Affine3d pose(rotation, tvec);
visualizer.setWidgetPose("top", pose);
visualizer.setWidgetPose("bottom", pose);
最后用一个循环，不断显示可视化窗口。中间暂停1 毫秒，以响应鼠标事件：
// 循环显示
while(cv::waitKey(100)==-1 && !visualizer.wasStopped()) {
visualizer.spinOnce(1, // 暂停1 毫秒
true); // 重绘
}

3. 用标定相机实现三维重建

        当从多个视角观察同一个场景时，即使没有三维场景的任何信息，也可以重建三维姿态和结构。我们这次将利用不同视角下图像点之间的关系，计算出三维信息。

       相机的标定参数是能够获取到的，因此可以使用世界坐标系，还可以用它在相机姿态和对应点的位置之间建立一个物理约束。这里引入一个新的数学实体——本质矩阵。简单来说，本质矩阵就是经过标定的基础矩阵。

// 找出image1 和image2 之间的本质矩阵
cv::Mat inliers;
cv::Mat essential = cv::findEssentialMat(points1, points2,
Matrix, // 内部参数 相当于给出了内参
cv::RANSAC,
0.9, 1.0, // RANSAC 方法
inliers); // 提取到的内点

      将匹配到的同名点调用triangulate 函数，计算三角剖分点的位置：

// 根据旋转量R 和平移量T 构建投影矩阵
cv::Mat projection2(3, 4, CV_64F); // 3×4 的投影矩阵
rotation.copyTo(projection2(cv::Rect(0, 0, 3, 3)));
translation.copyTo(projection2.colRange(3, 4));
// 构建通用投影矩阵
cv::Mat projection1(3, 4, CV_64F, 0.); // 3×4 的投影矩阵
cv::Mat diag(cv::Mat::eye(3, 3, CV_64F));
diag.copyTo(projection1(cv::Rect(0, 0, 3, 3)));
// 用于存储内点
std::vector<cv::Vec2d> inlierPts1;
std::vector<cv::Vec2d> inlierPts2;
// 创建输入内点的容器，用于三角剖分
int j(0);
for (int i = 0; i < inliers.rows; i++) {
if (inliers.at<uchar>(i)) {
inlierPts1.push_back(cv::Vec2d(points1[i].x, points1[i].y));
inlierPts2.push_back(cv::Vec2d(points2[i].x, points2[i].y));
}
}
// 矫正并标准化图像点
std::vector<cv::Vec2d> points1u;
cv::undistortPoints(inlierPts1, points1u,
cameraMatrix, cameraDistCoeffs);
std::vector<cv::Vec2d> points2u;
cv::undistortPoints(inlierPts2, points2u,
cameraMatrix, cameraDistCoeffs);
// 三角剖分
std::vector<cv::Vec3d> points3D;
triangulate(projection1, projection2,
points1u, points2u, points3D);

        图中有两个相机，相对的旋转量为R，平移量为T。平移向量T 刚好连接了两个相机的投影中心点。此外，向量x 连接第一个相机的中心点与一个图像点，向量x'连接第二个相机的中心点
与对应的图像点。因为这两个相机之间的移动量是已知的，所以可以用与第二个相机的相对值来表示x 的方向，记为Rx。仔细观察图像点的几何形状，就能发现T、Rx 和x'在同一个平面上。这个关系可用数学公式表示

       由于噪声和数字化过程的影响，理想情况下应该相交的投影线在实际中一般不会相交。所以用最小二乘法就可以大致找到交点的位置。但这种方法无法重建无穷远处的点，因为它们的齐次坐标的第4 个元素为0，而不是假定的1。还有一点很重要，三维重建只受限于缩放因子。如果要测量实际尺寸，就必须预先确定至少一个长度值，例如两个相机之间的实际距离或者画面中某个物体的实际高度。

4. 计算立体图像的深度

        人类用两只眼睛观察三维世界，装上两台相机后，机器也可以看到三维世界，这就是立体视觉。在同一个设备上安装两台相机，让它们观察同一个场景，并且两者之间有固定的基线（即相机之间的距离），就构成了一个立体视觉装置。

        两台相机之间只有水平方向的平移，因此它们的所有对极线都是水平方向的。这意味着所有关联点的y 坐标都是相同的，只需要在一维的线条上寻找匹配项即可。关联点x 坐标的差值则取决于点的深度。无穷远处的点对应图像点的坐标相同，都是(x, y)，而它们离装置越近，x 坐标的差值就越大，这时计算差值x -x'（注意要除以s 以符合齐次坐标系），并分离出z 坐标，可得到：

// 1 计算单应变换矫正量
cv::Mat h1, h2;
cv::stereoRectifyUncalibrated(points1, points2,
fundamental,
image1.size(), h1, h2);// 2 用变换实现图像矫正
cv::Mat rectified1;
cv::warpPerspective(image1, rectified1, h1, image1.size());
cv::Mat rectified2;
cv::warpPerspective(image2, rectified2, h2, image1.size());// 3 计算视差
cv::Mat disparity;
cv::Ptr<cv::StereoMatcher> pStereo =
cv::StereoSGBM::create(0, // 最小视差32, // 最大视差5); // 块的大小
pStereo->compute(rectified1, rectified2, disparity);