ubuntu18.04编译 BALM

发布于:2024-06-21 ⋅ 阅读:(19) ⋅ 点赞:(0)


紧接着上次编译完HBA,直接来编译BALM,发现基本上没有错误,但是在BARGE_test.cpp和PA_test.cpp中ParamSO3会出现错误:

错误改正主要参照:

Ceres Manifolds/LocalParameterization 类_ceres::localparameterization-CSDN博客

错误的原因主要是:

ceres2.2.0版本中没有localParammeters了,需要换成ManFold。具体的需要把BARGE_test.cpp和PA_test.cpp中的ParamSO3都换一下,

truct ParamSO3 : public ceres::Manifold 
{
  virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const 
  {
    Eigen::Map<const Eigen::Vector3d> tangent(x);
    Eigen::Map<const Eigen::Vector3d> drot(delta);
    Eigen::Map<Eigen::Vector3d> out(x_plus_delta);

    out = Log(Exp(tangent) * Exp(drot));

    return true;
  }

  // LocalParameterization 内为 ComputeJacobian()。
  // virtual bool ComputeJacobian(const double *x, double *jacobian) const 
  virtual bool PlusJacobian(const double *x, double *jacobian) const
  {
    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

   // 增加
    bool Minus(const double *y, const double *x, double *y_minus_x) const {return true;};
bool MinusJacobian(const double *x, double *jacobian) const  {return true;};

  // int Manifold::AmbientSize() const;  流形嵌入到的 环境空间 维度。 LocalParameterization 内为 GlobalSize()。
  // virtual int GlobalSize() const { return 3; };
  virtual int AmbientSize() const { return 3;};

  // int Manifold::TangentSize() const;  流形/切空间 维度。 LocalParameterization 内为 LocalSize()。
  // virtual int LocalSize() const { return 3; };
  int  TangentSize() const  { return 3; };
};

同时这个也需要换:

       ceres::Manifold *parametrization = new ParamSO3();

最后总结一下:

ParamSO3中需要把对应的基类和函数换成对应的 基类和函数, 但是minus和minusJacobian原函数中没有这个,因此需要在原函数中增加这两个函数,但是我只在里面增加了return true, 编译是没有问题了,但是不知道能不能计算得到正确的结果。

执行

roslaunch balm2 benchmark_realworld.launch

出现:

出现:

优化后得到结果: