使用 ceres 求解器構建旋轉優化問題

Question

我正在嘗試校准相機和激光雷達，為此我想在 Ceres 中進行一些簡單的優化。

我有以下代碼：

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

#include <Eigen/Core>
#include "camera_3Dlaser_calib/util.hpp"


struct RotationResidual 
{
    RotationResidual(Eigen::Vector3f camera_normal, Eigen::Vector3f lidar_normal) : 
                     camera_normal(camera_normal), lidar_normal(lidar_normal) {}
    
    template <typename T>
    bool operator()(const T* const R, T* residual) const 
    {
        residual[0] = 
        Eigen::abs(camera_normal.cross((R[0] * lidar_normal.transpose()).transpose()));
        return true;
    }
 
 private:
    const Eigen::Vector3f camera_normal;
    const Eigen::Vector3f lidar_normal;
};


class CeresOptimizer
{

public: 
    std::shared_ptr<std::vector<calibData>> calibDataVec;

    CeresOptimizer(std::shared_ptr<std::vector<calibData>> calibDataVec)
                    : calibDataVec(calibDataVec)
    {}

    void Optimize()
    {
        Eigen::Matrix3f RotationMat = {};
        ceres::Problem problem;
        
        for (int i = 0; i < calibDataVec->size(); ++i) 
        {
            Eigen::Vector3f lidar_normal = {calibDataVec->at(i).cb_plane_coefficients[0],
                                            calibDataVec->at(i).cb_plane_coefficients[1],
                                            calibDataVec->at(i).cb_plane_coefficients[2]};
            problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<RotationResidual, 9, 3, 3>(
                    new RotationResidual(calibDataVec->at(i).cb_camera_normals, lidar_normal)),
                    NULL, &RotationMat);
        }
        
        ceres::Solver::Options options;
        options.max_num_iterations = 25;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
        Solve(options, &problem, &summary);
        std::cout << summary.BriefReport() << "\n";
        std::cout << "Final   RotMat: " << RotationMat << "\n";
    }


};

我收到以下錯誤：

In file included from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp: In member function ‘void CeresOptimizer::Optimize()’:
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:50:39: error: no matching function for call to ‘ceres::Problem::AddResidualBlock(ceres::AutoDiffCostFunction<RotationResidual, 9, 3, 3>*, NULL, Eigen::Matrix3f*)’
   50 |                     NULL, &RotationMat);
      |                                       ^
In file included from /usr/local/include/ceres/ceres.h:60,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:1,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/usr/local/include/ceres/problem.h:240:19: note: candidate: ‘template<class ... Ts> ceres::internal::ResidualBlock* ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFunction*, double*, Ts* ...)’
  240 |   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
      |                   ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/problem.h:240:19: note:   template argument deduction/substitution failed:
In file included from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:50:27: note:   cannot convert ‘& RotationMat’ (type ‘Eigen::Matrix3f*’ {aka ‘Eigen::Matrix<float, 3, 3>*’}) to type ‘double*’
   50 |                     NULL, &RotationMat);
      |                           ^~~~~~~~~~~~
In file included from /usr/local/include/ceres/ceres.h:60,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:1,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/usr/local/include/ceres/problem.h:252:19: note: candidate: ‘ceres::internal::ResidualBlock* ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFunction*, const std::vector<double*>&)’
  252 |   ResidualBlockId AddResidualBlock(
      |                   ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/problem.h:255:35: note:   no known conversion for argument 3 from ‘Eigen::Matrix3f*’ {aka ‘Eigen::Matrix<float, 3, 3>*’} to ‘const std::vector<double*>&’
  255 |       const std::vector<double*>& parameter_blocks);
      |       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~
/usr/local/include/ceres/problem.h:259:19: note: candidate: ‘ceres::internal::ResidualBlock* ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFunction*, double* const*, int)’
  259 |   ResidualBlockId AddResidualBlock(CostFunction* cost_function,
      |                   ^~~~~~~~~~~~~~~~
/usr/local/include/ceres/problem.h:259:19: note:   candidate expects 4 arguments, 3 provided
In file included from /usr/local/include/ceres/internal/autodiff.h:152,
                 from /usr/local/include/ceres/autodiff_cost_function.h:130,
                 from /usr/local/include/ceres/ceres.h:37,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:1,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/usr/local/include/ceres/internal/variadic_evaluate.h: In instantiation of ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, std::false_type, std::integer_sequence<int, Indices ...>) [with Functor = RotationResidual; T = double; int ...Indices = {0, 1}; std::false_type = std::integral_constant<bool, false>]’:
/usr/local/include/ceres/internal/variadic_evaluate.h:79:30:   required from ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, const void*) [with ParameterDims = ceres::internal::ParameterDims<false, 3, 3>; Functor = RotationResidual; T = double]’
/usr/local/include/ceres/internal/variadic_evaluate.h:108:45:   required from ‘bool ceres::internal::VariadicEvaluate(const Functor&, const T* const*, T*) [with ParameterDims = ceres::internal::ParameterDims<false, 3, 3>; Functor = RotationResidual; T = double]’
/usr/local/include/ceres/autodiff_cost_function.h:207:55:   required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = RotationResidual; int kNumResiduals = 9; int ...Ns = {3, 3}]’
/usr/local/include/ceres/autodiff_cost_function.h:200:8:   required from here
/usr/local/include/ceres/internal/variadic_evaluate.h:57:17: error: no match for call to ‘(const RotationResidual) (const double* const&, const double* const&, double*&)’
   57 |   return functor(input[Indices]..., output);
      |          ~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:14:10: note: candidate: ‘template<class T> bool RotationResidual::operator()(const T*, T*) const’
   14 |     bool operator()(const T* const R, T* residual) const
      |          ^~~~~~~~
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:14:10: note:   template argument deduction/substitution failed:
In file included from /usr/local/include/ceres/internal/autodiff.h:152,
                 from /usr/local/include/ceres/autodiff_cost_function.h:130,
                 from /usr/local/include/ceres/ceres.h:37,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:1,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/usr/local/include/ceres/internal/variadic_evaluate.h:57:17: note:   deduced conflicting types for parameter ‘T’ (‘double’ and ‘const double’)
   57 |   return functor(input[Indices]..., output);
      |          ~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/internal/variadic_evaluate.h: In instantiation of ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, std::false_type, std::integer_sequence<int, Indices ...>) [with Functor = RotationResidual; T = ceres::Jet<double, 6>; int ...Indices = {0, 1}; std::false_type = std::integral_constant<bool, false>]’:
/usr/local/include/ceres/internal/variadic_evaluate.h:79:30:   required from ‘bool ceres::internal::VariadicEvaluateImpl(const Functor&, const T* const*, T*, const void*) [with ParameterDims = ceres::internal::ParameterDims<false, 3, 3>; Functor = RotationResidual; T = ceres::Jet<double, 6>]’
/usr/local/include/ceres/internal/variadic_evaluate.h:108:45:   required from ‘bool ceres::internal::VariadicEvaluate(const Functor&, const T* const*, T*) [with ParameterDims = ceres::internal::ParameterDims<false, 3, 3>; Functor = RotationResidual; T = ceres::Jet<double, 6>]’
/usr/local/include/ceres/internal/autodiff.h:351:39:   required from ‘bool ceres::internal::AutoDifferentiate(const Functor&, const T* const*, int, T*, T**) [with int kNumResiduals = 9; ParameterDims = ceres::internal::ParameterDims<false, 3, 3>; Functor = RotationResidual; T = double]’
/usr/local/include/ceres/autodiff_cost_function.h:210:69:   required from ‘bool ceres::AutoDiffCostFunction<CostFunctor, kNumResiduals, Ns>::Evaluate(const double* const*, double*, double**) const [with CostFunctor = RotationResidual; int kNumResiduals = 9; int ...Ns = {3, 3}]’
/usr/local/include/ceres/autodiff_cost_function.h:200:8:   required from here
/usr/local/include/ceres/internal/variadic_evaluate.h:57:17: error: no match for call to ‘(const RotationResidual) (const ceres::Jet<double, 6>* const&, const ceres::Jet<double, 6>* const&, ceres::Jet<double, 6>*&)’
In file included from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:14:10: note: candidate: ‘template<class T> bool RotationResidual::operator()(const T*, T*) const’
   14 |     bool operator()(const T* const R, T* residual) const
      |          ^~~~~~~~
/home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:14:10: note:   template argument deduction/substitution failed:
In file included from /usr/local/include/ceres/internal/autodiff.h:152,
                 from /usr/local/include/ceres/autodiff_cost_function.h:130,
                 from /usr/local/include/ceres/ceres.h:37,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/include/camera_3Dlaser_calib/Optimizer.hpp:1,
                 from /home/rgh/autnms/calibration/catkin_ws/src/camera_3Dlaser_calib/src/Optimizer.cpp:1:
/usr/local/include/ceres/internal/variadic_evaluate.h:57:17: note:   deduced conflicting types for parameter ‘T’ (‘ceres::Jet<double, 6>’ and ‘const ceres::Jet<double, 6>’)
   57 |   return functor(input[Indices]..., output);
      |          ~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~
make[2]: *** [CMakeFiles/camera_3Dlaser_calib.dir/build.make:102: CMakeFiles/camera_3Dlaser_calib.dir/src/Optimizer.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:295: CMakeFiles/camera_3Dlaser_calib.dir/all] Error 2
make: *** [Makefile:141: all] Error 2

找不到我做錯了什么。

Answer 1

我有一個類似的問題。 我認為創建成本時的數字 function 代表輸入/輸出量 arguments，但它們代表每個參數的維度 -> dim(residual), dim(a), dim(b)... 因為你只有const T* const R, T* residual你的電話應該是這樣的：

new ceres::AutoDiffCostFunction<RotationResidual, dim(residual), dim(R)>

Answer 2

您正在傳遞一個指向 RotationMat 作為輸入的指針。 通過 RotationMat.data()。