Ceres Solver: как сделать преобразование типа данных и операции в операторе

Я пытаюсь использовать 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
};

Две строки кодов закомментированы в функции «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 с этими двумя строками кода (НЕ ИСПОЛЬЗУЕТСЯ в функции, только копирование и передача), я получу неправильные результаты:
результаты после передачи данных

Может ли кто-нибудь помочь мне выяснить, в чем проблема и что мне делать, если я хочу выполнить какую-то операцию с параметрами, чтобы получить правильные соответствующие точки? Спасибо!