参考系列2：优化库——ceres（三）实战案例

最新推荐文章于 2023-07-21 17:51:09 发布
最新推荐文章于 2023-07-21 17:51:09 发布
阅读量1.4k 收藏 11
点赞数 1
分类专栏： 视觉SLAM基础理论 文章标签： 自动驾驶
原文链接：https://blog.csdn.net/jdy_lyy/article/details/119361019
版权

实战案例一





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入门例子

#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;
}

案例一深入理解：

Part 1: Cost Function

（1）AutoDiffCostFunction（自动求导） - - - - - - - 情况一

  • 构造 代价函数结构体（例如：struct CostFunctor），在其结构体内对 模板括号() 重载，定义残差；
  • 在重载的 () 函数 形参 中，最后一个为残差，前面几个为待优化状态量；
struct CostFunctor {
    template<typename T>
    bool operator()(const T *const x, T *residual) const {
        residual[0] = T(10.0) - x[0]; // r(x) = 10 - x
        return true;
    }
};
  • 创建代价函数的实例，对于模板参数的数字，第一个为残差的维度，后面几个为待优化状态量的维度
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

（2） NumericDiffCostFunction （数值求导） - - - - - - - 情况二

  • 数值求导法 也是构造 代价函数结构体，但在重载 括号() 时没有用模板
struct CostFunctorNum {
    bool operator()(const double *const x, double *residual) const {
        residual[0] = 10.0 - x[0]; // r(x) = 10 - x
        return true;
    }
};
  • 并且在实例化代价函数时也稍微有点区别，多了一个模板参数 ceres::CENTRAL
ceres::CostFunction *cost_function;
cost_function =
new ceres::NumericDiffCostFunction<CostFunctorNum, ceres::CENTRAL, 1, 1>(new CostFunctorNum);

（3） 自定义 CostFunction (自己求残差)  - - - - - - - 情况三

  • 构建一个 继承自 ceres::SizedCostFunction<1,1> 的类，同样，对于模板参数的数字，第一个为残差的维度，后面几个为待优化状态量的维度；
  • 重载 虚函数 virtual bool Evaluate(double const* const* parameters, double *residuals, double **jacobians) const，根据 待优化变量，实现 残差和雅克比矩阵的计算；
class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
    virtual ~SimpleCostFunctor() {};

    virtual bool Evaluate(
      double const* const* parameters, double *residuals, double** jacobians) const {
        const double x = parameters[0][0];

        residuals[0] = 10 - x; // r(x) = 10 - x

        if(jacobians != NULL && jacobians[0] != NULL) {
            jacobians[0][0] = -1; // r'(x) = -1
        }

        return true;
    }
};

Part 2: AddResidualBlock

  • 声明 ceres::Problem problem;
  • 通过 AddResidualBlock 将 代价函数(cost function)、损失函数(loss function) 和 待优化状态量 添加到 problem
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

ceres::Problem problem;
problem.AddResidualBlock(cost_function, NULL, &x);

Part 3: Config & Solve

配置求解器，并计算，输出结果

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;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";

Simple Example Code

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

struct CostFunctor {
    template<typename T>
    bool operator()(const T *const x, T *residual) const {
        residual[0] = 10.0 - x[0]; // f(x) = 10 - x
        return true;
    }
};

struct CostFunctorNum {
    bool operator()(const double *const x, double *residual) const {
        residual[0] = 10.0 - x[0]; // f(x) = 10 - x
        return true;
    }
};

class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
    virtual ~SimpleCostFunctor() {};

    virtual bool Evaluate(
      double const* const* parameters, double *residuals, double **jacobians) const {
        const double x = parameters[0][0];

        residuals[0] = 10 - x; // f(x) = 10 - x

        if(jacobians != NULL && jacobians[0] != NULL) {
            jacobians[0][0] = -1; // f'(x) = -1
        }

        return true;
    }
};

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

    double x = 0.5;
    const double initial_x = x;

    ceres::Problem problem;

    // Set up the only cost function (also known as residual)
    ceres::CostFunction *cost_function;

    // auto-differentiation
//    cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

     //numeric differentiation
//    cost_function =
//    new ceres::NumericDiffCostFunction<CostFunctorNum, ceres::CENTRAL, 1, 1>(
//      new CostFunctorNum);

    cost_function = new SimpleCostFunctor;

    // 添加代价函数cost_function和损失函数NULL，其中x为状态量
    problem.AddResidualBlock(cost_function, NULL, &x);

    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";
    std::cout << "x : " << initial_x << " -> " << x << "\n";

    return 0;
}

实战案例二、基于李代数的视觉SLAM位姿优化（解析求导）

以下内容来源与参考：

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

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

（1）残差（预测值 - 观测值）

（2）雅克比矩阵

（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]); // p2d[i], p3d[i]是已知参数；
    problem.AddResidualBlock(cost_function, NULL, se3.data()); // 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. p2d[i], p3d[i]是已知参数；
  2.  se3.data()需要优化的参数，其和 Evaluate()中的 parameters 对应；

他人是一面镜子，保持谦虚的态度
关注
  • 1
    点赞
  • 踩
  • 11
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
专栏目录
ceres-solver库和所依赖的工具包
10-20
该压缩包包括ceres-solver1.11.0、glog-0.3.3、gflags-master和camke3.4.0，有需要的可以下载看看。
ceres使用方式
weixin_43662239的博客
11-22 1727
Ceres Solver提供了三种求导方式：自动求导、数值求导和解析求导。 自动求导方式 自动求导是通过定义一个仿函数，然后传给AutoDiffCostFunction，就可以让Ceres自己去求导。 所谓仿函数，其实是一个类，只不过这个类的作用像函数，所以叫仿函数。原理就是类实现了operator()函数。 自动求导仿函数实现的operator()函数必须是模板函数，因为Ceres内部求导要用到。 可直接理解T为double。 \\构造仿函数 struct AutoCostFunctor { Au
ceres-solver
04-06
Ceres官网最新版本
ceres库（2.1.0版）
07-31
ceres库（2.1.0版）
ceres-solver库
10-30
使用cmake和VS2013编译，生成的ceres-solver，包含eigen、gflags和glog三个库，可直接在VS debug下使用
ceres 优化库使用要点记录
liukang10的博客
07-21 348
ceres 库安装使用，修改优化LBA过程
Ceres安装以及使用
qq_43526137的博客
09-28 817
Ceres
linux cmake 编译总结
feidaji的博客
12-04 155
通常的CMakeLists结构是： cmake_minimum_required( VERSION 2.8 ) project( projectname ) find_packages( , ) include_directories( ) add_executable( , ) target_link_libraries( , ) 前两行不谈，看看find_packages find_package写法 对于OpenCV我们是这样写的： find_package( OpenCV 3.1 R
安装ceres库的避坑指南（附官方文档）
slampai的博客
11-21 2548
安装ceres库的常见问题汇总及解决方法
Clang、G++、 Cmake编译Ceres测试代码
DrElaine的博客
06-13 2968
在ubuntu16.04下安装ceres之后，想要编译使用ceres库的测试代码，对于cmake, g++,clang:一、cmake直接按照别的博客中说的方法，在Cmaklist( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )find_package( Ceres REQUIRED )include_director...
ceres2.0+vs2019编译的动态库
09-11
ceres2.0在windows下使用vs2019编译的动态库。包含glog、gflags、suitsparse、eigen依赖。
谷歌开源非线性优化库ceres-solver
04-27
Ceres官网上的文档非常详细地介绍了其具体使用方法，相比于另外一个在slam中被广泛使用的图优化库G2O，ceres的文档可谓相当丰富详细（没有对比就没有伤害，主要是G2O资料太少了，对比起来就显得ceres的很多）。...
windows版本的ceres优化库
04-21
本文件是在windows系统下编译成功和使用的ceres优化库，编译环境为win7，64位，vs2012。
ceres-solver：大规模非线性优化库
02-27
Ceres Solver是一个开源C ++库，用于建模和解决大型，复杂的优化问题。 它是一个功能丰富，成熟且性能卓越的库，自2010年以来已在Google的生产环境中使用。Ceres Solver可以解决两种问题。 具有边界约束的非线性...
pose-graph-optimization:使用Ceres Solver进行2D姿态图优化
05-13
姿势图优化使用Ceres Solver进行2D姿态图优化的示例代码。依存关系本征3.3或更高版本Ceres Solver 1.12.0或更高版本Gflags 2.2.0或更高版本带有matplotlib的Python建造$ git clone ...
Ceres库的入门介绍
ABC1225741797的博客
06-02 1975
Ceres库的入门介绍 一、背景知识简介 Ceres库主要由于求解优化问题，通过对测量获取的不十分可靠的数据和理想化的预测模型进行优化处理获取尽可能接近真实值的结果。本文通过俩个实例来详细说明ceres在优化问题和最小二乘法的求解步骤。 二、优化 1/2*(10−x)2 通过优化求解ｘ值使得1/2(10−x)^2最小。 Ceres问题求解主要分成一下三部分： 1. 构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现； 2. 通过上一步的代价函
2021-07-01C++
weixin_38216573的博客
07-01 72
目录 ceres中仿函数的使用 ceres中仿函数的使用 https://www.cnblogs.com/gary-guo/p/9767777.html //构建代价函数结构体，abc为待优化参数，residual为残差 struct CURVE_FITTING_COST { CURVE_FITTING_COST(double x, double y): _x(x), _y(y){} template<typename T&g..
Ceres 学习 安装、编译、求解Options参数
xiaoma_bk的博客
10-10 2662
ceres 学习 安装、编译、使用1. 基础1.1 下载1.2 Linux 安装1.2.1 依赖安装1.2. 2 安装1.2.3 测试1.3 简易使用1.3.1 cmake中1.3.2 ros package1.4 指定版本+本地安装1.5 使用步骤2. Ceres的Options详解2.1 常用参数2.2 优化方法无关参数2.3 信任区域2.4 线搜索 1. 基础 Ceres solver 是...
ceres学习笔记(一)：三种求导方式
weixin_43835047的博客
03-08 145
ceres学习笔记
structstruct
最新发布
09-06
struct是一种在编程中用于定义自定义数据类型的关键字。它允许我们将不同的变量组合在一起，形成一个新的数据结构。通过使用struct，我们可以将相关的数据项打包在一起，以便更方便地管理和操作。 在C语言中，我们可以使用struct关键字来定义一个结构体，并指定其成员变量的类型和名称。下面是一个示例： ```c struct Person { char name[20]; int age; float height; }; ``` 在这个例子中，我们定义了一个名为Person的结构体，它包含三个成员变量：name、age和height。name的类型是字符数组，age的类型是整数，height的类型是浮点数。 通过定义结构体，我们可以创建该结构体类型的变量，并使用点操作符来访问和修改其成员变量。例如： ```c struct Person person1; strcpy(person1.name, "John"); person1.age = 25; person1.height = 1.75; ``` 这样，我们就创建了一个名为person1的Person类型变量，并为其成员变量赋值。我们可以通过person1.name、person1.age和person1.height来访问和修改相应的成员变量。 希望能够帮到你！如果有更多问题，请随时提问。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值