Class RelativePoseError

tinynav/cpp/pose_graph_solver.cpp:11–47

T_j_i @ (T_w_i)^-1 @ T_w_j = I

// T_j_i @ (T_w_i)^-1 @ T_w_j = I
class RelativePoseError {
public:
    RelativePoseError(const Eigen::Matrix4d& relative_pose, Eigen::Vector3d translation_weight, Eigen::Vector3d rotation_weight)
        : relative_j_i_translation_(Eigen::Vector3d::Zero()), relative_j_i_rotation_lie_algebra_(Eigen::Vector3d::Zero()), translation_weight_(translation_weight), rotation_weight_(rotation_weight) {
            relative_j_i_translation_ = relative_pose.block<3, 1>(0, 3);
            Eigen::Matrix<double, 3, 3> relative_j_i_rotation = relative_pose.block<3, 3>(0, 0);
            ceres::RotationMatrixToAngleAxis(relative_j_i_rotation.data(), relative_j_i_rotation_lie_algebra_.data());
        }
    template<typename T>
    bool operator()(const T* camera_i, const T* camera_j, T* residuals) const {
        using TMatrix3 = Eigen::Matrix<T, 3, 3>;
        using TVector3 = Eigen::Matrix<T, 3, 1>;
        TVector3 translation_i = Eigen::Map<const TVector3>(camera_i);
        TVector3 rotation_i = Eigen::Map<const TVector3>(camera_i + 3);
        TVector3 translation_j = Eigen::Map<const TVector3>(camera_j);
        TVector3 rotation_j = Eigen::Map<const TVector3>(camera_j + 3);
        TMatrix3 R_i;
        ceres::AngleAxisToRotationMatrix(rotation_i.data(), R_i.data());
        TMatrix3 R_j;
        ceres::AngleAxisToRotationMatrix(rotation_j.data(), R_j.data());
        TMatrix3 relative_j_i_rotation;
        TVector3 relative_j_i_rotation_lie_algebra_T = relative_j_i_rotation_lie_algebra_.cast<T>();
        ceres::AngleAxisToRotationMatrix(relative_j_i_rotation_lie_algebra_T.data(), relative_j_i_rotation.data());
        TMatrix3 rotation_loss = relative_j_i_rotation * R_i.transpose() * R_j;
        ceres::RotationMatrixToAngleAxis(rotation_loss.data(), residuals + 3);
        Eigen::Map<TVector3> rotation_residuals_map(residuals + 3);
        rotation_residuals_map = rotation_weight_.cast<T>().asDiagonal() * rotation_residuals_map;
        TVector3 translation_residuals = relative_j_i_rotation * R_i.transpose() * (translation_j - translation_i) + relative_j_i_translation_;
        Eigen::Map<TVector3> translation_residuals_map(residuals);
        translation_residuals_map = translation_weight_.cast<T>().asDiagonal() * translation_residuals;
        return true;
    }
    Eigen::Vector3d relative_j_i_translation_;
    Eigen::Vector3d relative_j_i_rotation_lie_algebra_;
    Eigen::Vector3d translation_weight_;
    Eigen::Vector3d rotation_weight_;
};

std::unordered_map<int64_t, py::array_t<double>> pose_graph_solve(
    std::unordered_map<int64_t, py::array_t<double>> camera_poses,