SLAM--三角化ceres 优化

Abstract

三角化常用方法：https://blog.csdn.net/qq_42995327/article/details/118917141
针对双目相机三角化优化目标，重投影误差损失函数
$$Loss=||u_1-\frac{1}{s_1}K{P}_1|{|}^2+||u_2-\frac{1}{s_2}K{T}_{21}{P}_1|{|}^2$$

定义残差

class TriangulationOptimization {
    public:
    TriangulationOptimization(const Eigen::Vector4d& qvec,
                              const Eigen::Vector3d& tvec,
                              double *intrinsics,
                              double *distortion,
                              const Eigen::Vector2d& point2D)
        :   qw_(qvec(0)),
            qx_(qvec(1)),
            qy_(qvec(2)),
            qz_(qvec(3)),
            tx_(tvec(0)),
            ty_(tvec(1)),
            tz_(tvec(2)),
            k1(distortion[0]),
            k2(distortion[1]),
            k3(distortion[4]),
            p1(distortion[2]),
            p2(distortion[3]),
            observed_x_(point2D(0)),
            observed_y_(point2D(1)) {
                K[0] = intrinsics[0];
                K[1] = 0.0;
                K[2] = intrinsics[2];
                K[3] = 0.0;
                K[4] = intrinsics[4];
                K[5] = intrinsics[5];
                K[6] = 0.0;
                K[7] = 0.0;
                K[8] = 1.0;
            }

    static ceres::CostFunction* Create(const Eigen::Vector4d& qvec,
                                        const Eigen::Vector3d& tvec,
                                        double *intrinsics,
                                        double *distortion,
                                        const Eigen::Vector2d& point2D) {
        return (new ceres::AutoDiffCostFunction<
                TriangulationOptimization, 2, 3>(
            new TriangulationOptimization(qvec, tvec, intrinsics, distortion, point2D)));
    }

    template <typename T>
    bool operator()(const T* const point3D, T* residuals) const {
        const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)};

        // Rotate and translate.
        T projection[3];
        ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
        projection[0] += T(tx_);
        projection[1] += T(ty_);
        projection[2] += T(tz_);

        // Project to image plane.
        projection[0] /= projection[2];
        projection[1] /= projection[2];

        // Distort and transform to pixel coordinate.
        // T Sn = projection[0] * projection[0] + projection[1] * projection[1];
        // T deltaX =  T(2) * T(p1) * projection[0] * projection[1] + T(p2) * (Sn + T(2) * projection[0] * projection[0]) ;
        // T deltaY = T(p1) * (Sn + T(2) * projection[1] * projection[1]) + T(2) * T(p2) * projection[0] * projection[1] ;

        // T k_d = T(1) + ((T(k3) * Sn + T(k2)) * Sn + T(k1)) * Sn;
        // T pn[3] = {projection[0]*k_d + deltaX, projection[1]*k_d + deltaY, T(1.0)}; 
        T pn[3] = {projection[0], projection[1], T(1.0)}; 
        T K_jet[9] = {T(K[0]),T(K[1]),T(K[2]),T(K[3]),T(K[4]),T(K[5]),T(K[6]),T(K[7]),T(K[8])};
        
        T *pi   = MulT(K_jet,pn,3,1);

        // Re-projection error.
        residuals[0] = pi[0] - T(observed_x_);
        residuals[1] = pi[1] - T(observed_y_);

        delete[] pi;

        return true;
    }

    private:
        const double qw_;
        const double qx_;
        const double qy_;
        const double qz_;
        const double tx_;
        const double ty_;
        const double tz_;
        const double k1;
        const double k2;
        const double k3;
        const double p1;
        const double p2;
        const double observed_x_;
        const double observed_y_;
        double K[3*3];
};

定义problem，添加残差

void TriangulatePoints(Eigen::Matrix3d K_left_inv, Eigen::Matrix3d K_right_inv, double *intrinsic1, double *distortion1,
    double *intrinsic2, double *distortion2, Eigen::Matrix3d R21, Eigen::Vector3d t21, const std::vector<CV::Point2d> solid_centersL, 
    const std::vector<CV::Point2d> solid_centersR, std::vector<CV::Point3f> &solid_pts3d)
{

    int num_pts = solid_centersL.size();
    solid_pts3d.resize(num_pts);

    double R[9] = {R21(0,0),R21(0,1),R21(0,2),R21(1,0),R21(1,1),R21(1,2),R21(2,0),R21(2,1),R21(2,2)};
    double q[4] = {0,0,0,0};
    MatrixToQuaternion((double *)(q), (double *)(R));
    Eigen::Vector4d qvec(q[0],q[1],q[2],q[3]);
    Eigen::Vector3d tvec(t21[0],t21[1],t21[2]);

    Eigen::Vector4d qvecL(1,0,0,0);
    Eigen::Vector3d tvecL(0,0,0);

    ceres::Solver::Options options;
    options.max_num_iterations = 200;
    options.function_tolerance=1.0e-8;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = false;
    Solver::Summary summary;

    // Do not use multiple thread to process this part.
    // #pragma omp parallel for
    for (int i = 0; i < num_pts; ++i)
    {
        Eigen::Vector3d             pixel_left(solid_centersL[i].x, solid_centersL[i].y, 1);
        Eigen::Vector3d             pixel_right(solid_centersR[i].x, solid_centersR[i].y, 1);
        Eigen::Matrix<double, 3, 2> A;
        A.block(0, 0, 3, 1) = -R21 * K_left_inv * pixel_left;
        A.block(0, 1, 3, 1) = K_right_inv * pixel_right;

        Eigen::Vector2d d = (A.transpose() * A).inverse() * A.transpose() * t21;
        Eigen::Vector3d p1 = d[0] * K_left_inv * pixel_left;

        Eigen::Vector2d pts2dLeft(solid_centersL[i].x, solid_centersL[i].y);
        Eigen::Vector2d pts2dRight(solid_centersR[i].x, solid_centersR[i].y);
        Problem problem;
        CostFunction* cost_function_right = TriangulationOptimization::Create(qvec,tvec,intrinsic2,distortion2,pts2dRight);
        CostFunction* cost_function_left = TriangulationOptimization::Create(qvecL,tvecL,intrinsic1,distortion1,pts2dLeft);
        problem.AddResidualBlock(cost_function_left,NULL,p1.data());
        problem.AddResidualBlock(cost_function_right,NULL,p1.data());

        Solve(options, &problem, &summary);

        solid_pts3d[i] = CV::Point3f(p1[0],p1[1],p1[2]);
    }
}

p1就是最后优化的三维坐标。

其他

头文件及引用

#include "ceres/ceres.h"
#include "ceres/autodiff_cost_function.h"
#include "ceres/rotation.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;

其他函数

template <typename T>
T *MulT(T *A, T *B, const int M, const int N)
{
    T *C = new T[M * N]{};
    for (int i = 0; i < M; i++)
    {
        for (int j = 0; j < N; j++)
        {
            for (int k = 0; k < M; k++)
            {
                C[i * N + j] += A[i * M + k] * B[k * N + j];
            }
        }
    }
    return C;
}