C++ ceres solver：如何在operator中进行数据类型转换和操作_C++_Eigen_Ceres Solver

C++ ceres solver：如何在operator中进行数据类型转换和操作

c++

C++ ceres solver：如何在operator中进行数据类型转换和操作,c++,eigen,ceres-solver,C++,Eigen,Ceres Solver,我正在尝试使用ceres solver优化点云转换过程通过遵循ceres solver教程中的示例，我获得了优化过程的简单工作版本。但是，当我试图进一步修改操作符中的函数（在MyCostFunctor类中）时，结果完全错误（解算器收敛，但给出错误的结果）。我发现问题是由两行代码引起的，我试图将参数从模板类型T转换为特征矩阵类型代码如下： template<typename T> inline void DataTransfer(const T* input, Eigen::Mat

我正在尝试使用ceres solver优化点云转换过程

通过遵循ceres solver教程中的示例，我获得了优化过程的简单工作版本。但是，当我试图进一步修改操作符中的函数（在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
};

模板内联
无效数据传输（常量T*输入、特征：：矩阵和输出）{
对于（int i=0；i<12；++i）{
输出[i]=输入[i]；
}
}
模板内联
T*GetCorrespondingPoint（常数T*rot，常数PtT pt）{
//**错误**
//本征：：矩阵参数=本征：：矩阵：：零（12）；
//数据传输（rot、参数）；
//*****
T结果[3]；
结果[0]=rot[0]*T（pt（0））+rot[1]*T（pt（1））+rot[2]*T（pt（2））+rot[9]；
结果[1]=rot[3]*T（pt（0））+rot[4]*T（pt（1））+rot[5]*T（pt（2））+rot[10]；
结果[2]=rot[6]*T（pt（0））+rot[7]*T（pt（1））+rot[8]*T（pt（2））+rot[11]；
返回结果；
}
//实现剩余r=x-y的代价函子。
//其中x=R*x'+T或添加更多操作，如x=C*逆（（R*x'）*A+T*B），A、B、C是相关向量或矩阵
模板
类MyCostFunctor{
公众：
MyCostFunctor（PtT&x、PtT&y、int pt\U id）
：x_ux，y_ux，idx_x（pt_id）{
}
模板
布尔运算符（）{
//数据转换
T*rslt；
rslt=GetCorrespondingPoint（参数，x_）；
残差[0]=T（rslt[0]-y_0））；
残差[1]=T（rslt[1]-y_（1））；
剩余[2]=T（rslt[2]-y_2））；
返回true；
}
私人：
PtT x；//震源点
PtT y；//目标点
int idx；//源点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;
}

#包括
#包括
#包括
#包括
#包括
#包括“ceres/ceres.h”
#包括“glog/logging.h”
#包括“ceres/dynamic\u autodiff\u cost\u function.h”
使用ceres:：NumericDiffCostFunction；
使用ceres:：AutoDiffCostFunction；
使用ceres:：sizedcost函数；
使用ceres:：CENTRAL；
使用ceres：：CostFunction；
使用ceres：：问题；
使用ceres：：Solver；
使用ceres：：Solve；
int main（int argc，字符**argv）{
google:：InitGoogleLogging（argv[0]）；
//1.样本数据设置
std：：向量模型；
模型_pts.clear（）；
std：：矢量目标点；
目标点清除（）；
模型参数推回（特征：：Vector3d（10.0,10.0,10.0））；
模型点推回（特征：：矢量3D（20.0,10.0,10.0））；
模型参数推回（特征：：Vector3d（10.0,20.0,10.0））；
模型参数推回（特征：：Vector3d（10.0,10.0,20.0））；
目标点推回（特征：：矢量3D（40.0,40.0,40.0））；
目标点推回（特征：：矢量3D（40.0,30.0,40.0））；
目标点推回（特征：：矢量3D（30.0,40.0,40.0））；
目标点推回（特征：：矢量3D（40.0,40.0,30.0））；
///设置模型和目标点配对的索引
std：：向量pt_idx；
pt_idx.推回（0）；
pt_idx.推回（1）；
pt_idx.推回（2）；
pt_idx.推回（3）；
//打印pts
std:：cout残差的代码使用行主形式的矩阵rot
，而Eigen默认为列主形式：
谢谢你的建议。但似乎这两种方法都会导致相同的问题。问题似乎不是由我如何定义和执行数据类型传输引起的（通过在DataTransfer函数中打印输出，我得到了预期的结果，这些参数的值至少在该函数中是正确的）。