0

我正在尝试使用 ceres 求解器来优化点云转换过程。

通过遵循 ceres 求解器教程中的示例，我得到了优化过程的简单工作版本。但是，当我尝试进一步修改运算符中的函数（在 MyCostFunctor 类中）时，结果完全错误（求解器收敛，但给出了错误的结果）。我发现问题是由我试图将参数从模板类型 T 转换为特征矩阵类型的两行代码引起的。

以下是代码：

template<typename T> inline
void DataTransfer(const T* input, Eigen::Matrix<T, Eigen::Dynamic, 1>& output) {
    for (int i = 0; i < 12; ++i) {
        output[i] = input[i];
    }
}

template<typename T, typename PtT> inline
T* GetCorrespondingPoint(const T* rot, const PtT pt) {
    //**!!!!!!!!!!! Error !!!!!!!!!!!**
    //Eigen::Matrix<T, Eigen::Dynamic, 1> param_vecs = Eigen::Matrix<T, Eigen::Dynamic, 1>::Zero(12);
    //DataTransfer<T>(rot, param_vecs);
    // **!!!!!!!!!! Error !!!!!!!!!!!**

    T result[3];
    result[0] = rot[0] * T(pt(0)) + rot[1] * T(pt(1)) + rot[2] * T(pt(2)) + rot[9];
    result[1] = rot[3] * T(pt(0)) + rot[4] * T(pt(1)) + rot[5] * T(pt(2)) + rot[10];
    result[2] = rot[6] * T(pt(0)) + rot[7] * T(pt(1)) + rot[8] * T(pt(2)) + rot[11];

    return result;
}

// A cost functor that implements the residual r = x - y.
// where x = R*x' + T or add more operations such as x = C*inverse((R*x')*A + T*B), A, B, C are related vectors or matrices
template<typename PtT>
class MyCostFunctor {
public:
    MyCostFunctor(PtT& x, PtT& y, int pt_id)
        :x_(x), y_(y), idx_(pt_id) {
    }

    template<typename T>
    bool operator()(const T* const params, T* residual) const {
        // Data transformation
        T* rslt;
        rslt = GetCorrespondingPoint<T, PtT>(params, x_);

        residual[0] = T(rslt[0] - y_(0));
        residual[1] = T(rslt[1] - y_(1));
        residual[2] = T(rslt[2] - y_(2));

        return true;
    }

private:
    PtT x_;     // source point
    PtT y_;     // target point
    int idx_;   // source point idx
};

这两行代码在函数“GetCorrespondingPoint”中被注释掉了。

主函数代码如下：

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <Eigen/Dense>

#include "ceres/ceres.h"
#include "glog/logging.h"
#include "ceres/dynamic_autodiff_cost_function.h"

using ceres::NumericDiffCostFunction;
using ceres::AutoDiffCostFunction;
using ceres::SizedCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

int main(int argc, char** argv){
    google::InitGoogleLogging(argv[0]);

    // 1. Sample Data Set Up
    std::vector<Eigen::Vector3d> model_pts;
    model_pts.clear();
    std::vector<Eigen::Vector3d> target_pts;
    target_pts.clear();

    model_pts.push_back(Eigen::Vector3d(10.0, 10.0, 10.0));
    model_pts.push_back(Eigen::Vector3d(20.0, 10.0, 10.0));
    model_pts.push_back(Eigen::Vector3d(10.0, 20.0, 10.0));
    model_pts.push_back(Eigen::Vector3d(10.0, 10.0, 20.0));

    target_pts.push_back(Eigen::Vector3d(40.0, 40.0, 40.0));
    target_pts.push_back(Eigen::Vector3d(40.0, 30.0, 40.0));
    target_pts.push_back(Eigen::Vector3d(30.0, 40.0, 40.0));
    target_pts.push_back(Eigen::Vector3d(40.0, 40.0, 30.0));

    /// Set up the index for pairing the model and target points
    std::vector<int> pt_idx;
    pt_idx.push_back(0);
    pt_idx.push_back(1);
    pt_idx.push_back(2);
    pt_idx.push_back(3);

    // print pts
    std::cout << "Model pts\t\tTarget pts\n";
    for (int i = 0; i < model_pts.size(); ++i) {
        std::cout << model_pts[i](0) << " " << model_pts[i](1) << " " << model_pts[i](2) << "\t\t\t"
            << target_pts[i](0) << " " << target_pts[i](1) << " " <<     target_pts[i](2) << "\n";
    }

    // Parameter Set up
    double params[12];
    for (int i = 0; i < 12; ++i) {
        params[i] = 1.0;
    }

    // Set up the problem
    int num_pts = target_pts.size();
    Problem problem;
    for (int i = 0; i < num_pts; ++i) {
        problem.AddResidualBlock(
            new AutoDiffCostFunction<MyCostFunctor<Eigen::Vector3d>, 3, 12>(new MyCostFunctor<Eigen::Vector3d>(model_pts[i], target_pts[i], pt_idx[i])), NULL,&params[0]);
    }

    // Set the solver options
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;

    // Run the solver!
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    std::cout << summary.FullReport() << "\n\n";

    // print results
    std::cout << "test results: \n";
    for (int i = 0; i < model_pts.size(); ++i) {
        Eigen::Vector3d pt;
        pt(0) = params[0]*model_pts[i](0) + params[1]*model_pts[i](1) + params[2]*model_pts[i](2) + params[9];
        pt(1) = params[3]*model_pts[i](0) + params[4]*model_pts[i](1) + params[5]*model_pts[i](2) + params[10];
        pt(2) = params[6]*model_pts[i](0) + params[7]*model_pts[i](1) + params[8]*model_pts[i](2) + params[11];
        std::cout << pt(0) << " " << pt(1) << " " << pt(2) << "\n";
    }

    return 0;
}

如果我注释掉这两行，我会得到正确的结果： 数据传输前的结果

但是，当我尝试使用这两行代码将参数转换为 Eigen 格式时（函数中未使用，仅复制和传输），我会得到错误的结果： 数据传输后的结果

谁能帮我弄清楚问题出在哪里，如果我想对参数进行一些操作以获得正确的对应点，我该怎么办？谢谢！

4

1 回答 1

0

您的残差代码使用rot行主要形式的矩阵，而 Eigen 默认为列主要形式：

https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html

于 2018-08-01T22:44:35.513 回答