一个多视图三角化的入门小例子

        本例子可以当作一个多视图三角化的入门小例子，结合项目和slam十四讲ch7的学习，以及参考了https://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html

以及https://github.com/zhaoxuhui/bundle-adjustment

        现在拍摄了8张照片，照片里面包含了一个aruco mark board以及一个marker，

        那么可以直接用mark 板估计相机位姿    std::vector<cv::Mat> transformationMatrix_Vector;，从而得到相机在mark板坐标系的位姿，以后就把mark板当作世界坐标系。

        也可以用cv::aruco::DICT_ARUCO_ORIGINAL检测出那个单独的mark的中心的像素坐标

    detect_board_one(camMatrix, distCoeffs, photo_1, transformationMatrix_Vector.at(0), center_vector.at(0));
    detect_board_one(camMatrix, distCoeffs, photo_2, transformationMatrix_Vector.at(1), center_vector.at(1));
    detect_board_one(camMatrix, distCoeffs, photo_3, transformationMatrix_Vector.at(2), center_vector.at(2));
    detect_board_one(camMatrix, distCoeffs, photo_4, transformationMatrix_Vector.at(3), center_vector.at(3));

        好了，现在我们有了相机外参T，相机内参已知，目标点（mark中心）的像素坐标

        我们就可以用opencv的三角化求目标点的3D位置了，

    cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

        其中T1为第一张图片的相机位姿，T2为第二张相机位姿，pts_1为目标点在第一张图片的归一化平面位置，就是把我们得到的像素坐标转到归一化平面，pts_2为目标点在第2张图片，pts_4d就是我们需要得到的3D位置，不过需要把最后一维归一后，取前三维数。

        结果如下

手动三角化的结果RESULTRESULTRESULTRESULTRESULTRESULTRESULT：
[0.294733, 0.17403, 0.000187977]

        此3d坐标是mark中心在mark板坐标系下的位置。

        前面只用了前2张图片做了三角化，现在我们需要综合8张图片信息用g2o库优化，

        函数需要一个相机位姿T数组、目标点像素坐标points_2d数组、相机内参K、需要估计的目标点3D位置XYZ。

void bundleAdjustmentG2O(
    const VecSE3d& T,
    const VecVector2d& points_2d,
    const Mat& K,
    Eigen::Vector3d& XYZ)
{

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 6>> BlockSolverType;  // pose is 6, landmark is 3
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
    // 梯度下降方法，可以从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    // vertex             //[0.294733, 0.17403, 0.000187977]真实值
    //Eigen::Vector3d  initialEstimate = Eigen::Vector3d::UnitX(); // 单位向量 (1, 0, 0)   NO
    //Eigen::Vector3d  initialEstimate = Eigen::Vector3d (1, 1, 1); // OK
    Eigen::Vector3d  initialEstimate = Eigen::Vector3d(0, 0, 0); // OK

    摄像头位姿节点添加
    for (size_t i = 0; i < T.size(); ++i)
    {
        auto Ti = T[i];

        // 创建一个g2o::SE3Quat对象
        g2o::SE3Quat g2o_se3quat;

        // 从Sophus::SE3d对象中提取平移向量和旋转矩阵
        Eigen::Vector3d translation = Ti.translation();
        Eigen::Matrix3d rotation = Ti.rotationMatrix();

        // 使用提取的信息填充g2o::SE3Quat对象
        g2o_se3quat.setRotation(Eigen::Quaterniond(rotation));
        g2o_se3quat.setTranslation(translation);


        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i+1);

        v->setFixed(true);
        
        v->setEstimate(g2o_se3quat);
        optimizer.addVertex(v);

    }



    /// <summary>
    /// 特征点节点添加
    /// </summary>
    /// <param name="T"></param>
    /// <param name="points_2d"></param>
    /// <param name="K"></param>
    /// <param name="XYZ"></param>
    g2o::VertexSBAPointXYZ* vertex_pose = new g2o::VertexSBAPointXYZ();
    vertex_pose->setId(0);
    vertex_pose->setMarginalized(false);
    vertex_pose->setEstimate(initialEstimate);
    optimizer.addVertex(vertex_pose);



    // K
    Eigen::Matrix3d K_eigen;
    K_eigen <<
        K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
        K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
        K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

    ///摄像头参数设置
        //相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters(K.at<double>(1, 1), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0);
    camera->setId(0);
    optimizer.addParameter(camera);


    // edges
    int index = 1;
    for (size_t i = 0; i < points_2d.size(); ++i)
    {
        Eigen::Vector2d p2d = points_2d[i];


        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(0)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(i + 1)));
        edge->setMeasurement(p2d);
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        edge->setRobustKernel(new g2o::RobustKernelHuber());

        optimizer.addEdge(edge);

        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
    cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl << endl;
    XYZ = vertex_pose->estimate();
}

        函数核心是optimizer.addVertex(v);优化器添加了8个相机位姿节点、1个目标点3D位置、8个边也就是映射关系，其中        v->setFixed(true);我把8个相机位姿节点设置了不变量，其实我也想把相机位姿和3D点一起优化，但是我v->setFixed(false);程序就会出错，目前不知道原因。

        最终的效果如上图，两种结果欧式距离大概在1mm

pose estimated by g2o =
  0.293432
  0.172331
0.00136134

手动三角化的结果RESULTRESULTRESULTRESULTRESULTRESULTRESULT：
[0.294733, 0.17403, 0.000187977]

        本工程已经上传github，地址为https://github.com/applefb/little-sfm

        依赖opencv contrib 4.6.0，需要viz模块

        以及g2o库，建议用vcpkg安装，比自己cmake安装方便。