优化库——ceres(三)实战案例

实战案例

1.1 CmakeLists.txt配置

cmake_minimum_required(VERSION 2.8)
project(ceres)

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

add_executable(test test.cpp)
target_link_libraries(test ${CERES_LIBRARIES})

1.2 示例:ceres入门例子

一个简单的求解 min ⁡ x ( 10 − x ) 2 \min _{x} (10-x)^{2} minx(10x)2 的优化问题代码如下:

#include<iostream>
#include<ceres/ceres.h>

using namespace std;
using namespace ceres;

//第一部分:构建代价函数,重载()符号,仿函数的小技巧
struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = T(10.0) - x[0];
     return true;
   }
};

//主函数
int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);

  // 寻优参数x的初始值,为5
  double initial_x = 5.0;
  double x = initial_x;

  // 第二部分:构建寻优问题
  Problem problem;
  //使用自动求导,将之前的代价函数结构体传入,第一个1是输出维度,即残差的维度,第二个1是输入维度,即待寻优参数x的维度。
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); 
  //向问题中添加误差项,本问题比较简单,添加一个就行。
  problem.AddResidualBlock(cost_function, NULL, &x); 

  //第三部分: 配置并运行求解器
  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
  options.minimizer_progress_to_stdout = true;//输出到cout
  Solver::Summary summary;//优化信息
  Solve(options, &problem, &summary);//求解!!!

  std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
  //最终结果
  std::cout << "x : " << initial_x << " -> " << x << "\n";
  return 0;
}

1.3 应用:曲线拟合(使用的是自动求导,不用写雅克比)

以下内容来源与参考:

一文助你Ceres 入门——Ceres Solver新手向全攻略

问题:拟合非线性函数的曲线

y = e 3 x 2 + 2 x + 1 y=e^{3 x^{2}+2 x+1} y=e3x2+2x+1


整个代码的思路还是先构建代价函数结构体,然后在[0,1]之间均匀生成待拟合曲线的1000个数据点,加上方差为1的白噪声,数据点用两个vector储存(x_data和y_data),然后构建待求解优化问题,最后求解,拟合曲线参数。
(PS. 本段代码中使用OpenCV的随机数产生器,要跑代码的同学可能要先装一下OpenCV)

#include<iostream>
#include<opencv2/core/core.hpp>
#include<ceres/ceres.h>
using namespace std;
using namespace cv;

//构建代价函数结构体,abc为待优化参数,residual为残差。
struct CURVE_FITTING_COST
{
  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
  template <typename T>
  bool operator()(const T* const abc,T* residual)const
  {
    residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
    return true;
  }
  const double _x,_y;
};

//主函数
int main()
{
  //参数初始化设置,abc初始化为0,白噪声方差为1(使用OpenCV的随机数产生器)。
  double a=3,b=2,c=1;
  double w=1;
  RNG rng;
  double abc[3]={0,0,0};

//生成待拟合曲线的数据散点,储存在Vector里,x_data,y_data。
  vector<double> x_data,y_data;
  for(int i=0;i<1000;i++)
  {
    double x=i/1000.0;
    x_data.push_back(x);
    y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w));
  }

//反复使用AddResidualBlock方法(逐个散点,反复1000次)
//将每个点的残差累计求和构建最小二乘优化式
//不使用核函数,待优化参数是abc
  ceres::Problem problem;
  for(int i=0;i<1000;i++)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
        new CURVE_FITTING_COST(x_data[i],y_data[i])
      ),
      nullptr,
      abc
    );
  }

//配置求解器并求解,输出结果
  ceres::Solver::Options options;
  options.linear_solver_type=ceres::DENSE_QR;
  options.minimizer_progress_to_stdout=true;
  ceres::Solver::Summary summary;
  ceres::Solve(options,&problem,&summary);
  cout<<"a= "<<abc[0]<<endl;
  cout<<"b= "<<abc[1]<<endl;
  cout<<"c= "<<abc[2]<<endl;
return 0;
}
}

1.4 应用: 基于李代数的视觉SLAM位姿优化(解析求导)

以下内容来源与参考:

Ceres-Solver 从入门到上手视觉SLAM位姿优化问题

下面以 基于李代数的视觉SLAM位姿优化问题 为例,介绍 Ceres Solver 的使用。

(1)残差(预测值 - 观测值)
r ( ξ ) = K exp ⁡ ( ξ ∧ ) P − u r(\xi)=K \exp \left(\xi^{\wedge}\right) P-u r(ξ)=Kexp(ξ)Pu

(2)雅克比矩阵
J = ∂ r ( ξ ) ∂ ξ = [ f x Z ′ 0 − X ′ f x Z ′ 2 − X ′ Y ′ f x Z ′ 2 f x + X ′ 2 f x Z ′ 2 − Y ′ f x Z ′ 0 f y Z ′ − Y ′ f y Z 2 − f y − Y ′ 2 f y Z ′ 2 X ′ Y ′ f y Z ′ 2 X ′ f y Z ′ ] ∈ R 2 × 6 J=\frac{\partial r(\xi)}{\partial \xi} \\ =\left[\begin{array}{ccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{X^{\prime} f_{x}}{Z^{\prime 2}} & -\frac{X^{\prime} Y^{\prime} f_{x}}{Z^{\prime 2}} & f_{x}+\frac{X^{\prime 2} f_{x}}{Z^{\prime 2}} & -\frac{Y^{\prime} f_{x}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{Y^{\prime} f_{y}}{Z^{2}} & -f_{y}-\frac{Y^{\prime 2} f_{y}}{Z^{\prime 2}} & \frac{X^{\prime} Y^{\prime} f_{y}}{Z^{\prime 2}} & \frac{X^{\prime} f_{y}}{Z^{\prime}} \end{array}\right] \in \mathbb{R}^{2 \times 6} J=ξr(ξ)=[Zfx00ZfyZ2XfxZ2YfyZ2XYfxfyZ2Y2fyfx+Z2X2fxZ2XYfyZYfxZXfy]R2×6

(3)核心代码

代价函数的构造:

//应该是重投影误差,这样的话才会有残差2维的,优化变量是6维度的
class BAGNCostFunctor : public ceres::SizedCostFunction<2, 6> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW //对齐
	//? 二维,三维? 第一个是像素观测值   第二个是准备用来重投影的三维观测值
    BAGNCostFunctor(Eigen::Vector2d observed_p, Eigen::Vector3d observed_P) :
            observed_p_(observed_p), observed_P_(observed_P) {}

    virtual ~BAGNCostFunctor() {}

    virtual bool Evaluate(
        //参数 残差  雅克比
      double const* const* parameters, double *residuals, double **jacobians) const {

        Eigen::Map<const Eigen::Matrix<double,6,1>> T_se3(*parameters);

        Sophus::SE3 T_SE3 = Sophus::SE3::exp(T_se3);

        Eigen::Vector3d Pc = T_SE3 * observed_P_;

        Eigen::Matrix3d K;
        double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
        K << fx, 0, cx, 0, fy, cy, 0, 0, 1;
        
        //! 计算残差
        Eigen::Vector2d residual =  observed_p_ - (K * Pc).hnormalized();

        residuals[0] = residual[0];
        residuals[1] = residual[1];

        if(jacobians != NULL) {

            if(jacobians[0] != NULL) {
                // 2*6的雅克比矩阵
                Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> J(jacobians[0]);

                double x = Pc[0];
                double y = Pc[1];
                double z = Pc[2];

                double x2 = x*x;
                double y2 = y*y;
                double z2 = z*z;

                J(0,0) =  fx/z;
                J(0,1) =  0;
                J(0,2) = -fx*x/z2;
                J(0,3) = -fx*x*y/z2;
                J(0,4) =  fx+fx*x2/z2;
                J(0,5) = -fx*y/z;
                J(1,0) =  0;
                J(1,1) =  fy/z;
                J(1,2) = -fy*y/z2;
                J(1,3) = -fy-fy*y2/z2;
                J(1,4) =  fy*x*y/z2;
                J(1,5) =  fy*x/z;
            }
        }

        return true;
    }

private:
    const Eigen::Vector2d observed_p_;
    const Eigen::Vector3d observed_P_;
};

构造优化问题,并求解相机位姿:

Sophus::Vector6d se3;

//在当前problem中添加代价函数残差块,损失函数为NULL采用默认的最小二乘误差即L2范数,优化变量为 se3
ceres::Problem problem;
for(int i=0; i<n_points; ++i) {
    ceres::CostFunction *cost_function;
    cost_function = new BAGNCostFunctor(p2d[i], p3d[i]);
    problem.AddResidualBlock(cost_function, NULL, se3.data());
}

ceres::Solver::Options options;
options.dynamic_sparsity = true;
options.max_num_iterations = 100; //迭代100次
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = true;
options.dogleg_type = ceres::SUBSPACE_DOGLEG;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";

std::cout << "estimated pose: \n" << Sophus::SE3::exp(se3).matrix() << std::endl;
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值