c++ - 使用 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 ‘&amp; 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

找不到我做错了什么。

score 0 · Accepted Answer

我有一个类似的问题。我认为创建成本函数时的数字代表输入/输出参数的数量，但它们代表每个参数的维度 -> dim(residual), dim(a), dim(b)... 因为你只有const T* const R, T* residual你的调用应该是这样的：

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

score -1 · Accepted Answer

-1

您正在传递一个指向 RotationMat 作为输入的指针。通过 RotationMat.data()。

于 2021-03-31T23:32:28.810 回答

c++ - 使用 ceres 求解器构建旋转优化问题

2 回答 2

Related

Reference