#include <opencv2/opencv.hpp>
#include <iostream>
#include <map>
#include <vector>

// 定义相机节点
struct CameraNode {
    int cameraId;  // 相机 ID
    cv::Mat R;     // 旋转矩阵
    cv::Mat t;     // 平移向量
    cv::Mat K;     // 内参矩阵
};

// 定义三维点节点
struct PointNode {
    int pointId;  // 三维点 ID
    cv::Point3f X;  // 三维点坐标
};

// 定义观测边
struct ObservationEdge {
    int cameraId;  // 相机 ID
    int pointId;   // 三维点 ID
    cv::Point2f x;  // 观测到的二维点坐标
};

// 定义 Bundle Adjustment 图结构
class BundleAdjustmentGraph {
public:
    // 添加相机节点
    void addCameraNode(const CameraNode& camera) {
        cameras[camera.cameraId] = camera;
    }

    // 添加三维点节点
    void addPointNode(const PointNode& point) {
        points[point.pointId] = point;
    }

    // 添加观测边
    void addObservationEdge(const ObservationEdge& edge) {
        observations.push_back(edge);
    }

    // 获取相机节点
    CameraNode getCameraNode(int cameraId) {
        return cameras[cameraId];
    }

    // 获取三维点节点
    PointNode getPointNode(int pointId) {
        return points[pointId];
    }

    // 获取观测边
    std::vector<ObservationEdge> getObservationEdges() {
        return observations;
    }

private:
    std::map<int, CameraNode> cameras;  // 相机节点集合
    std::map<int, PointNode> points;    // 三维点节点集合
    std::vector<ObservationEdge> observations;  // 观测边集合
};