1 GNSS边定义添加

1-1 边定义

// GNSS边定义 添加
class G2O_TYPES_SBA_API GNSSConstraint_sRt_Edge : public  BaseUnaryEdge<3, Vector3D, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    GNSSConstraint_sRt_Edge(){};
    GNSSConstraint_sRt_Edge(Eigen::Matrix4d T_VO_to_GNSS_ENU ) : _T_VO_to_GNSS_ENU(T_VO_to_GNSS_ENU) {}

    bool read(std::istream &in) {} //no use
  
    bool write(std::ostream &out) const {}// no use

    void computeError()  {
      const VertexSE3Expmap* v = static_cast<const VertexSE3Expmap*>(_vertices[0]);
     
        /*
        // 在优化器创建之前
        CameraParameters* camera_params = new CameraParameters(fx, Vector2D(cx, cy), 0.0);
        camera_params->setId(0);  // 给参数分配一个唯一ID
        */
        //通过parameter(0)获取相机参数
        //const CameraParameters * cam
        //  = static_cast<const CameraParameters *>(parameter(0));

        SE3Quat cam_pose = v->estimate(); // cw-ENU全局坐标  to_homogeneous_matrix
        const  Eigen::Matrix3d R_cw = cam_pose.rotation().toRotationMatrix(); // 世界到相机
        const  Eigen::Vector3d t_cw = cam_pose.translation();
         
        const  Eigen::Matrix3d R_wc = R_cw.transpose() ;// 相机到世界
        const  Eigen::Vector3d t_wc = -R_cw.transpose() * t_cw;// 并不是真的世界点坐标
 
        Eigen::Matrix3d R_cw_to_gw =_T_VO_to_GNSS_ENU.block<3, 3>(0, 0); // 包含了尺度
        Eigen::Vector3d t_cw_to_gw = _T_VO_to_GNSS_ENU.block<3, 1>(0, 3);//
 
 
        Eigen::Vector3d t_gw = R_cw_to_gw*t_wc+t_cw_to_gw;// GNSS-ENU全局坐标 enu世界点
      
        _error = _measurement - t_gw ; //_measurement GNSS 真值  调用setmeasurement（） 设置  Vector3d

      // Vector2D obs(_measurement);
      // _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    //  virtual void linearizeOplus() override {
    
    //     ::g2o::BaseUnaryEdge<3, Eigen::Vector3d, openvslam::optimize::g2o::se3::shot_vertex>::linearizeOplus();// 继承自动求导
    
    //  } 
    // void linearizeOplus() override {
    //     // 实现雅可比矩阵计算
    //     // 这里需要根据误差函数对位姿的导数来实现
    //     _jacobianOplusXi = Eigen::Matrix<double, 3, 6>::Zero();
        
    //     const VertexSE3Expmap* v = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    //     SE3Quat T = v->estimate();
        
    //     // 提取旋转矩阵
    //     const Eigen::Matrix3d R_cw = T.rotation().toRotationMatrix();
    //     const Eigen::Matrix3d R_cw_to_gw = _T_VO_to_GNSS_ENU.block<3, 3>(0, 0);
        
    //     // 位置误差对位姿的雅可比
    //     // ∂e/∂ξ = -R_cw_to_gw * R_wc^T
    //     _jacobianOplusXi.block<3, 3>(0, 0) = -R_cw_to_gw * R_cw.transpose();
        
    //     // 位置误差对平移的雅可比
    //     // ∂e/∂t = R_cw_to_gw
    //     _jacobianOplusXi.block<3, 3>(0, 3) = R_cw_to_gw;
    // }
    //CameraParameters * _cam;

  public:
    Eigen::Matrix4d _T_VO_to_GNSS_ENU = Eigen::Matrix4d::Identity(); // sRt第三方需要用得到的参数 真值Eigen::Matrix4d _T_VO_to_GNSS_ENU; // 真值
};

1-2 必须实现：类型注册宏

// 必须实现：类型注册宏
G2O_REGISTER_TYPE(GNSS_CONSTRAINT_SRT, GNSSConstraint_sRt_Edge); // 使用 G2O_REGISTER_TYPE 宏

sudo cp g2o.cpython-311-x86_64-linux-gnu.so /home/r9000k/anaconda3/envs/pyslam/lib/python3.11/site-packages

2 python

    // GNSS 边绑定  
    /*
    3 测量值的维度3
    Vector3D: 测量值（Observation / Measurement）的数据类型。
    VertexSE3Expmap: 第1个顶点的类型。一个使用李代数（在流形上）表示相机位姿的顶点。其优化变量是 6 维向量 (tx, ty, tz, qx, qy, qz)，前3个是平移，后3个是旋转的轴角（或 so(3) 李代数）。这是“相机位姿”。
    内部注册表唯一标识字符串 : GNSS_BaseUnaryEdge_3_Vector3D_VertexSE3Expmap
    */
    //templatedBaseUnaryEdge<3, Vector3D, VertexSE3Expmap>(m, "GNSS_BaseUnaryEdge_3_Vector3D_VertexSE3Expmap");
    py::class_<GNSSConstraint_sRt_Edge, BaseUnaryEdge<3, Vector3D, VertexSE3Expmap>>(m, "GNSSConstraint_sRt_Edge")
        .def(py::init<>(),"默认构造函数")
        .def(py::init<Eigen::Matrix4d>(), "带变换矩阵的构造函数",py::arg("T_VO_to_GNSS_ENU"))
        .def("compute_error", &GNSSConstraint_sRt_Edge::computeError) // 函数绑定
        //.def("linearize_oplus", &GNSSConstraint_sRt_Edge::linearizeOplus) // 函数绑定
        .def_readwrite("_T_VO_to_GNSS_ENU", &GNSSConstraint_sRt_Edge::_T_VO_to_GNSS_ENU) // 参数修改绑定 sRt第三方需要用得到的参数  赋值Eigen::Matrix4d
    ;

3 use

    # GNSS 自定义引用
    #gnss_edge=g2o.GNSSConstraint_sRt_Edge()
    
    T_VO_to_GNSS = np.eye(4)
    gnss_edge = g2o.GNSSConstraint_sRt_Edge(T_VO_to_GNSS)
    print("初始化参数",gnss_edge._T_VO_to_GNSS_ENU)
    
    T_VO_to_GNSS[0, 0] = 1.1  # 缩放因子
    T_VO_to_GNSS[1, 1] = 1.1
    T_VO_to_GNSS[2, 2] = 1.1
    T_VO_to_GNSS[0, 3] = 10.0  # 平移
    T_VO_to_GNSS[1, 3] = 20.0
    T_VO_to_GNSS[2, 3] = 30.0
    gnss_edge._T_VO_to_GNSS_ENU=T_VO_to_GNSS
    print("修改后参数",gnss_edge._T_VO_to_GNSS_ENU)
    
    gnss_edge.set_measurement([1,1,1]) #信息矩阵

posted on 2025-12-19 17:51 MKT-porter 阅读(0) 评论(0) 收藏举报

刷新页面返回顶部

1 GNSS边定义 添加

2 python

3 use

1 GNSS边定义添加