NoneG 2o оптимизировать две позы, которые разделяют очки?

Question

Я пытаюсь разобраться с настройкой связки с помощью библиотеки g2o.

У меня есть стереокамера, два набора совпадающих точек с каждой стороны и триангулированные 3d точки между ними.

У меня есть следующая функция, которая берет предварительно согласованные 2d точки, триангулированные 3d точки, камера позирует (одна в нуле, другая перемещается по базовой линии стерео) и оптимизирует все это. Проблема в том, что это не так. Результат совпадает с вводом. Куда я иду не так?

void SolveTwoFrames(KeyFrame &vpKF1, KeyFrame &vpKF2, std::vector<MapPoint> &allMapPoints, cv::Mat intrinsic, cv::Mat dist)
{

    int64 t0 = cv::getTickCount();

    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(true);
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;   
    linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();      
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));

    optimizer.setAlgorithm(solver);


    int num_cameras_ = 2;  
    //camera poses

    //frame 1
    g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
    g2o::SE3Quat squat;
    Eigen::Quaterniond qq = vpKF1.pose.get_orientation_quaternion();
    squat.setRotation(qq);
    Eigen::Vector3d pp = vpKF1.pose.get_position();
    squat.setTranslation(pp);
    vSE3->setEstimate(squat);
    vSE3->setId(0);
    vSE3->setFixed(true);
    optimizer.addVertex(vSE3);

    //frame2
    g2o::VertexSE3Expmap *vSE32 = new g2o::VertexSE3Expmap();
    g2o::SE3Quat squat2;
    Eigen::Quaterniond qq2 = vpKF2.pose.get_orientation_quaternion();
    squat2.setRotation(qq2);
    Eigen::Vector3d pp2 = vpKF2.pose.get_position();

    pp2.x() += 5; //ADDING NOISE TO TEST OPTIMISATION

    squat2.setTranslation(pp2);
    vSE32->setEstimate(squat2);
    vSE32->setId(1);
    optimizer.addVertex(vSE32);

    std::cout << "second pose: " << pp2.x() << " " << pp2.y() << " " << pp2.z() << " " << qq2.x() << " " << qq2.y() << " " << qq2.z() << " " << qq2.w() << std::endl;


    // set point vertex
    for (size_t i = 0; i < allMapPoints.size(); i++)
    {
        lvt_utils::MapPoint pMP = allMapPoints[i];

        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        g2o::Vector3 pp(pMP.m_position.x(), pMP.m_position.y(), pMP.m_position.z());
        vPoint->setEstimate(pp);
        vPoint->setId(i + num_cameras_);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);


        //SET EDGES
            //1 
            KeyFrame pKF = vpKF1;                   
            cv::Point2f kpUn = pKF.keypoints[i].pt;

            Eigen::Matrix<double, 2, 1> obs;
            obs << kpUn.x, kpUn.y;
            g2o::EdgeSE3ProjectXYZ *e = new g2o::EdgeSE3ProjectXYZ();

            int camera_id = pKF.id;
            int point_id = i + num_cameras_;

            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(point_id)));
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(camera_id)));
            e->setMeasurement(obs);
            e->setInformation(Eigen::Matrix2d::Identity());
            e->fx = pKF.fx;
            e->fy = pKF.fy;
            e->cx = pKF.cx;
            e->cy = pKF.cy;
            optimizer.addEdge(e);

            //2
            KeyFrame pKF2 = vpKF2;
            cv::Point2f kpUn2 = pKF2.keypoints[i].pt;

            Eigen::Matrix<double, 2, 1> obs2;
            obs2 << kpUn2.x, kpUn2.y;
            g2o::EdgeSE3ProjectXYZ *e2 = new g2o::EdgeSE3ProjectXYZ();

            camera_id = pKF2.id;

            e2->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(point_id)));
            e2->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(camera_id)));
            e2->setMeasurement(obs2);
            e2->setInformation(Eigen::Matrix2d::Identity());
            e2->fx = pKF2.fx;
            e2->fy = pKF2.fy;
            e2->cx = pKF2.cx;
            e2->cy = pKF2.cy;
            optimizer.addEdge(e2);


    }


    // start optimization
    optimizer.initializeOptimization();

    optimizer.optimize(50);


    for (int i = 0; i < num_cameras_; i++)
    {
        g2o::VertexSE3Expmap *pCamera = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(i));
        g2o::SE3Quat SE3quat = pCamera->estimate();

        g2o::Quaternion gQuat = SE3quat.rotation();
        g2o::Vector3 gTrans = SE3quat.translation();

        std::cout << i << " " << gTrans.x() << " " << gTrans.y() << " " << gTrans.z() << " " << gQuat.x() << " " << gQuat.y() << " " << gQuat.z() << " " << gQuat.w() << std::endl;


}