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

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

Источник

1 ответ

Ваш код для остатка использует матрицу rot в мажорной строке, в то время как Eigen по умолчанию принимает мажорную колонку:

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

Источник
Другие вопросы по тегам