使用Ceres求解RT变换矩阵

最新推荐文章于 2024-05-22 17:00:00 发布

Coding的叶子

最新推荐文章于 2024-05-22 17:00:00 发布

阅读量635

点赞数 3

分类专栏： C++ PCL实战文章标签：矩阵线性代数 c++ rt矩阵

本文为博主原创文章，未经博主允许不得转载。

本文链接：https://blog.csdn.net/suiyingy/article/details/136877271

版权

C++ PCL实战专栏收录该内容

8 篇文章 1 订阅

订阅专栏

【版权声明】
本文为博主原创文章，未经博主允许严禁转载，我们会定期进行侵权检索。

在计算机视觉和机器人领域，经常需要解决非线性优化问题来估计相机姿态或运动模型。Ceres Solver是一个开源的C++库，专门用于解决最小二乘问题，包括非线性优化问题。本文将介绍如何使用Ceres Solver来求解非线性优化问题，以估计旋转平移（RT）矩阵。

1. 引言

在计算机视觉中，我们经常需要根据已知的点对（比如特征点）来估计相机之间的变换关系，这个变换关系通常由一个旋转矩阵和一个平移向量组成。在本文中，我们将使用Ceres Solver来求解一个非线性优化问题，以估计从一组点对（x, y）中得到的旋转平移矩阵。

2. 定义代价函数

首先，我们需要定义一个Ceres所需的代价（误差）函数，这里我们定义了一个RTXYCostFunction类。这个代价函数的作用是计算预测点与真实点之间的误差，并将其作为优化目标。

3. 求解过程

接下来，我们定义了一个solve_RTXY函数，该函数使用Ceres Solver来执行优化过程。我们初始化优化参数并将代价函数添加到优化问题中。然后通过调用ceres::Solve函数进行优化，得到最优的旋转平移矩阵。

4. 主函数

在主函数中，我们创建了两组点对数据x_data和y_data，然后调用solve_RTXY函数来估计旋转平移矩阵。最后输出优化结果。

5. 示例程序

/*
更多python与C++技巧、三维算法、深度学习算法总结、大模型请关注我的博客，欢迎讨论与交流：https://blog.csdn.net/suiyingy，或”乐乐感知学堂“公众号。Python三维领域专业书籍推荐：《人工智能点云处理及深度学习算法》。
*/
#include <iostream>
#include <ceres/ceres.h>
#include <Eigen/Dense>

// 定义Ceres所需代价（误差）函数 CostFunction
class RTXYCostFunction {
public:
    RTXYCostFunction(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y)
        : x_(x), y_(y) {}

    template <typename T>
    bool operator()(const T* const params, T* residuals) const {
        int num_points = x_.cols();
        Eigen::Matrix<T, 3, 3> RT;
        RT << ceres::cos(params[0]), ceres::sin(params[0]), params[1],
              -ceres::sin(params[0]), ceres::cos(params[0]), params[2],
              T(0), T(0), T(1);

        for (int i = 0; i < num_points; ++i) {
            Eigen::Matrix<T, 3, 1> x_point = x_.col(i).template cast<T>();
            Eigen::Matrix<T, 3, 1> y_point = y_.col(i).template cast<T>();
            Eigen::Matrix<T, 3, 1> prediction = RT * x_point;
            
            for (int j = 0; j < 3; ++j) {
                residuals[3 * i + j] = prediction(j) - y_point(j);
            }
        }

        return true;
    }

private:
    const Eigen::MatrixXd x_;
    const Eigen::MatrixXd y_;
};

// 采用Ceres求解
Eigen::Matrix3d solve_RTXY(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y) {
    Eigen::Matrix3d M = Eigen::Matrix3d::Identity();
    double initial_params[3] = {1.0, 1.0, 1.0};
    ceres::Problem problem;
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<RTXYCostFunction, 12, 3>(new RTXYCostFunction(x, y));
    problem.AddResidualBlock(cost_function, nullptr, initial_params);
    
    ceres::Solver::Options options;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    if (summary.IsSolutionUsable()) {
        M << ceres::cos(initial_params[0]), ceres::sin(initial_params[0]), initial_params[1],
             -ceres::sin(initial_params[0]), ceres::cos(initial_params[0]), initial_params[2],
             0.0, 0.0, 1.0;
        
        std::cout << "Success: " << summary.IsSolutionUsable() << std::endl;
    } else {
        std::cout << "Optimization failed" << std::endl;
    }
    std::cout << "M matrix: " << std::endl << M << std::endl;
    return M;
}

int main() {
    Eigen::MatrixXd x_data(3, 4); // 3xN matrix
    Eigen::MatrixXd y_data(3, 4); // 3xN matrix

    x_data << 0.45001983, 0.14598769, 0.04914471, 0.57264854,
            0.99262731, 0.52823745, 0.46996383, 0.68941285,
            1.,         1.,         1.,         1.        ;

    y_data << 0.0470798, 0.99159949, 0.2678888, 0.1334882,
            0.84622237, 0.15186659, 0.70308835, 0.31705256,
            1.,         1.,         1.,         1.        ;
    Eigen::Matrix3d M = Eigen::Matrix3d::Identity();
    M = solve_RTXY(x_data, y_data);
    std::cout << "M matrix: " << std::endl << M << std::endl;
    return 0;
}