gtsam OrientedPlane3及OrientedPlane3Factor的使用

一、OrientedPlane3
构造函数是来自平面的方程系数：ax+by+cz+d=0的(a,b,c,d)
是有方向的。

比如一个平面，平行与xoz平面，在xoy平面的投影线段与x轴平行，处于y轴上方2处，这个平面的法向量可以是(0,1,0)：方向指向y轴正方向，也可以是(0,-1,0)，方向指向y轴负方向。具体定哪个，要看自己的使用需求。
不同的法向量，得出不同的参数d，比如第一个，其参数d=-2，第二个则是2。
即d的正负号，取决于原点在法向量的那一侧。

二、OrientedPlane3Factor
这个factor，用来描述从某个位姿观察到的一个平面，约束涉及的变量就是pose、plane，约束内容就是这个pose观察到的这个平面的方程。

构造函数的说明：
在这个pose下的平面方程的系数，而不是全局坐标系下的系数。

三、测试

using symbol_shorthand::X;
using symbol_shorthand::P;

void test_room_plane(){
    OrientedPlane3 p1(0,1,0,0);
    OrientedPlane3 p2(0,-1,0,1);
    OrientedPlane3 p3(-1,0,0,4);

    OrientedPlane3 p4(1,0,0,-3);
    OrientedPlane3 p5(0,1,0,-2);
    OrientedPlane3 p6(1,0,0,-2);

    OrientedPlane3 p7(0,-1,0,3);
    OrientedPlane3 p8(0,1,0,-2);
    OrientedPlane3 p9(-1,0,0,5);

    std::vector<OrientedPlane3> planes={p1,p2,p3,p4,p5,p6,p7,p8,p9};

    //朝x轴正方向
    gtsam::Pose3 pose1(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.5, 0.0));
    gtsam::Pose3 pose2(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.5, 0.5, 0.0));
    gtsam::Pose3 pose3(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0.5, 0.0));
    gtsam::Pose3 pose4(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(1.5, 0.5, 0.0));
    gtsam::Pose3 pose5(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(2, 0.5, 0.0));
    gtsam::Pose3 pose6(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(2.5, 0.5, 0.0));
    gtsam::Pose3 pose7(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(3, 0.5, 0.0));
    gtsam::Pose3 pose8(gtsam::Rot3::Ypr(0.0, 0.0, 0.0), Point3(3.5, 0.5, 0.0));

    //朝y轴正方向
    gtsam::Pose3 pose9(gtsam::Rot3::Ypr(M_PI/2, 0.0, 0.0), Point3(3.5, 0.5, 0.0));
    gtsam::Pose3 pose10(gtsam::Rot3::Ypr(M_PI/2, 0.0, 0.0), Point3(3.5, 1, 0.0));
    gtsam::Pose3 pose11(gtsam::Rot3::Ypr(M_PI/2, 0.0, 0.0), Point3(3.5, 1.5, 0.0));
    gtsam::Pose3 pose12(gtsam::Rot3::Ypr(M_PI/2, 0.0, 0.0), Point3(3.5, 2, 0.0));
    gtsam::Pose3 pose13(gtsam::Rot3::Ypr(M_PI/2, 0.0, 0.0), Point3(3.5, 2.5, 0.0));

    //朝x轴正方向
    gtsam::Pose3 pose14(gtsam::Rot3::Ypr(0, 0.0, 0.0), Point3(3.5, 2.5, 0.0));
    gtsam::Pose3 pose15(gtsam::Rot3::Ypr(0, 0.0, 0.0), Point3(4, 2.5, 0.0));
    gtsam::Pose3 pose16(gtsam::Rot3::Ypr(0, 0.0, 0.0), Point3(4.5, 2.5, 0.0));
    gtsam::Pose3 pose17(gtsam::Rot3::Ypr(0, 0.0, 0.0), Point3(5, 2.5, 0.0));

    std::vector<gtsam::Pose3> poses={pose1,
                                        pose2,
                                        pose3,
                                        pose4,
                                        pose5,
                                        pose6,
                                        pose7,
                                        pose8,
                                        pose9,
                                        pose10,
                                        pose11,
                                        pose12,
                                        pose13,
                                        pose14,
                                        pose15,
                                        pose16,
                                        pose17};

    //simple check
    auto p4_loc=p4.transform(pose9);
    pose9.print("pose9");
    p4.print("p4");
    p4_loc.print("p4_loc");


    //观测数据关联
    std::map<int,std::vector<int>> pose_id_to_plane_ids;//全部都加了1
    for(int i=1;i<9;i++){
        pose_id_to_plane_ids[i]={1,2,3};
    }
    for(int i=9;i<11;i++){
        pose_id_to_plane_ids[i]={1,3,4};
    }
    for(int i=11;i<13;i++){
        pose_id_to_plane_ids[i]={5,6,7};
    }
    for(int i=13;i<17;i++){
        pose_id_to_plane_ids[i]={6,7,8,9};
    }


    //构建factor
    gtsam::NonlinearFactorGraph graph;
    Values initialEstimate;

    Pose3 delta(Rot3::Ypr(0.0,0.0,0.0),Point3(0.05,-0.10,0.20));
    const auto x_p_noise = noiseModel::Isotropic::Sigma(3, 0.001);
    gtsam::Vector6 sigmas;
    sigmas<<0.3,0.3,0.3,0.1,0.1,0.1;
    auto pose_noise=noiseModel::Diagonal::Sigmas(sigmas);

    for(auto iter=pose_id_to_plane_ids.begin();iter!=pose_id_to_plane_ids.end();iter++){
        int pose_id=iter->first-1;
        std::vector<int> pids=iter->second;
        gtsam::Pose3 po=poses[pose_id];

        //pose加入噪声

        //pose加入graph先验
        graph.emplace_shared<PriorFactor<Pose3>>(X(pose_id), po, pose_noise);
        initialEstimate.insert(X(pose_id), po);

        //转换到pose的相对观测
        for(auto pid:pids){
            int planid=pid-1;

            auto pn=planes[planid];
            auto related_plane=pn.transform(po);

            //orientedplane factor
            Vector4 z=related_plane.planeCoefficients();
            OrientedPlane3Factor opf(z,x_p_noise,X(pose_id),P(planid));

            graph.add(opf);

            if(!initialEstimate.exists(P(planid))){
                initialEstimate.insert(P(planid),pn);
            }
        }   
    }



    GaussNewtonParams params;
    // params.setVerbosity("ERROR");
    params.setMaxIterations(20);
    params.setRelativeErrorTol(-std::numeric_limits<double>::max());
    // params.setErrorTol(-std::numeric_limits::max());
    params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

    Values result = GaussNewtonOptimizer(graph, initialEstimate, params).optimize();

    std::cout<<"\n\n--------optimized result-----------------\n";
    for(int i=0;i<poses.size();i++){
        if(result.exists(X(i))){
            // std::cout<<"pose"<
            std::string n="pose"+std::to_string(i+1);
            // result.at(X(i)).print(n);
            auto op=result.at<Pose3>(X(i));
            std::cout<<n<<": \nypr="<<op.rotation().ypr().transpose()<<"\npos="<<op.translation().transpose()<<std::endl;
            std::cout<<"\tdiff ypr="<<(op.rotation().ypr()-poses[i].rotation().ypr()).transpose()<<"\n"
            <<"\tdiff pos ="<<(op.translation()-poses[i].translation()).transpose()<<"\n";
            std::cout<<"\n";
        }
    }
    std::cout<<"\n\n";
    for(int i=0;i<planes.size();i++){
        if(result.exists(P(i))){
            // std::cout<<"pose"<
            std::string n="plane"+std::to_string(i+1)+"\n";
            auto op=result.at<OrientedPlane3>(P(i));
            result.at(P(i)).print(n);
            std::cout<<"diff coeff = "<<(op.planeCoefficients()-planes[i].planeCoefficients()).transpose()<<std::endl;
            std::cout<<"\n";
        }
    }

}