IPOPT和SNOPT均可，IPOPT是纯开源C++项目，而SNOPT最初是用FORTRAN开发的，现在也提供C++接口，不过是付费软件包。

公司买了nag，感觉效率挺高，就是调用函数都是c的风格，要花一些时间写c++的wrapper

G2o在很多环境中都能使用， C++,python,android,java等。

由于orb_slam是用C++写的，所以使用了g2o的C++版本。

不过，无论哪种版本，基本使用逻辑都相同。

1. 配置图的节点，配置图的边，配置所使用的优化算法
2. 调用优化函数进行优化

https://github.com/RainerKuemmerle/g2o/tree/pymem/python

看一个例子：https://github.com/RainerKuemmerle/g2o/blob/pymem/python/examples/ba_demo.py

本例子中，直接用g2o的SparseOptimazer来解决BA问题：

95行，生成一条边edge，边的功能是将3维点映射到图像坐标

96行，将边一个顶点设置为机器人位姿，即待优化求解的第一个量。

97行，将边的另一个顶点设置为三维点，即待求解的第二个量。

98行，将边的测量值设置为z,这个测量值，就是关键点的坐标。

我们用边的一个顶点机器人位姿把 边的另一个顶点：3维点坐标 映射到图片上的p点，计算p距离点z的距离做为误差。

所以z,称为测量值。

我们的目标就是求取合适的顶点值，即求取合适的机器人位姿和3维点，使p距离z点的距离近量接近0。

104行，将边加入到优化器中。

118行，利用算法进行优化，此句运行完，将会得到合适位姿和三维点。

120行至123行，计算投影误差。

也可以把SparseOptimazer直接封装为一个BA类：

其中，add_pose函数：把机器人位姿做为一种顶点；add_point：把三维点做为另一种顶点；add_edge：定义pose顶点和point顶点之间的投影关系，并提供真实二维点的坐标，即用ORB特征求得的关键点的坐标，即measurement, projection的measurement.

C++版本：

这个版本我们直接看ORB_SLAM中的调用方式。

我们看这个源码https://gitee.com/anjiang2020_admin/ORB_SLAM/blob/master/src/Optimizer.cc的第154行，PoseOptimization函数

设置机器人位姿为一种类型的顶点：帧顶点:VertexSE3Expmap，setFixed(false),不固定，可以改变的量。setId(0),顶点编号为0.

设置三维点为另一种类型的顶点：三维点顶点VertexSBAPointXYZ, setFixed(true),固定，三维点坐标不变。setId(I+1),顶点编号为：1，2，3，。。。N

设置三维点为另一种类型的顶点：三维点顶点VertexSBAPointXYZ, setFixed(true),固定，三维点坐标不变。setId(I+1),顶点编号为：1，2，3，。。。N

设置边，边的类型为：映射，即映射三维点的边， EdgeSE3ProjectXYZ

这里固定，就是已知量。不固定对应待优化的量。

213行，把三维点顶点设置为边的0位值顶点。

214行，把帧顶点设置为边的1位值顶点。

215行，把obs设置为边的测量值，即三维点的映射目标点。

Obs则是关键点去畸变后的坐标，如下：

配置完成后，就可以调用optimize(10),来进行优化了。词句写法与python写法一样。

下面，我们看一下PoseOptimizer函数的内是如何使用optimize的。

249行，for循环内使用了4次optimize,分别是optimize(10),optimize(10),optimize(7),optimize(5)

其实使用一次就能够得到机器人位姿了，为什么使用了4次呢？

因为第274行，我们希望优化的停止条件位为：误差小于某个值的边,这类边的数量，刚好小于10.

为了达到这个目的，我们 进行了4次优化，并且这4次优化后，可允许误差从9.210变化到了 5.911.

可允许误差的减小，意味着对优化结果要求越来越严格，满足要求的边的数量会减少。

282行，将最终的优化结果复制给pFrame->mTcw.

2023/12/29 23:39

第一次更新

? 已知目标函数，和约束，使用Ceres来估计最小化目标函数时，变量的取值。

? 本教程适合新手，直接上菜。

目标函数： 约束：

在上述约束下，求目标函数的最小值。

#include <ceres/ceres.h>

struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    residual[0] = x[0] * x[0] + y[0] * y[0];  // 目标函数 f(x, y)=x^2 + y^2
    return true;
  }
};

struct InequalityConstraint1 {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    // 不等式约束 1 <=x^3 + y^3 + xy <=2
    residual[0] = x[0] * x[0] * x[0] + y[0] * y[0] * y[0] + x[0] * y[0] - T(1);
    residual[1] = T(2) - (x[0] * x[0] * x[0] + y[0] * y[0] * y[0] + x[0] * y[0]);
    return true;
  }
};

struct InequalityConstraint2 {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    // 不等式约束 1 <=x^2 + y + xy <=2
    residual[0] = x[0] * x[0] + y[0] + x[0] * y[0] - T(1);
    residual[1] = T(2) - (x[0] * x[0] + y[0] + x[0] * y[0]);
    return true;
  }
};

struct EqualityConstraint {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    // 等式约束 x + y + xy=0
    residual[0] = x[0] + y[0] + x[0] * y[0];
    return true;
  }
};

int main() {
  ceres::Problem problem;

  double x = 1.5;  // 初始值
  double y = 0.5;  // 初始值

   //添加代价函数
  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CostFunctor, 1, 1, 1>(new CostFunctor),
      nullptr,
      &x, &y
  );

  // 添加第一个不等式约束
  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<InequalityConstraint1, 2, 1, 1>(new InequalityConstraint1),
      nullptr,
      &x, &y
  );

  // 添加第二个不等式约束
  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<InequalityConstraint2, 2, 1, 1>(new InequalityConstraint2),
      nullptr,
      &x, &y
  );

  // 添加等式约束
  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<EqualityConstraint, 1, 1, 1>(new EqualityConstraint),
      nullptr,
      &x, &y
  );

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

  std::cout << summary.FullReport() << std::endl;
  std::cout << "x: " << x << ", y: " << y << std::endl;

  return 0;
}

struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    residual[0] = x[0] * x[0] + y[0] * y[0];  // 目标函数 f(x, y)=x^2 + y^2
    return true;
  }
};

• 设置目标函数：
  
• c++ template <typename T> 使用模板函数，允许在不同数据类型（double,float)上运行改代码。
  
• ```c++ bool operator()(const T const x, const T const y, T* residual) 此处重载了运算符（）,x,y表示T类型的参数变量，T类型的residual用于储存残差. 采用数组的形式 x[0]的原因是为了支持多个参数的情况，而不仅仅局限于一个参数,例如，如果你的优化问题涉及到多个变量，可以通过 x[0]、x[1]、x[2]等来表示不同的参数。
  

在 Ceres Solver 中，结构体中的 operator() 函数被设计为返回 bool 类型的原因主要是用于指示残差计算的成功与否。具体来说，operator() 函数应该返回 true 表示成功计算残差，返回 false 表示计算失败。

这种设计的目的是让 Ceres Solver 在优化过程中能够判断是否成功计算了残差。如果返回 false，Ceres Solver 将认为当前优化变量的配置无效，可能会进行回退操作或采取其他策略。如果返回 true，则继续优化过程。

在典型的 Ceres Solver 的用法中，operator() 函数中的计算残差的逻辑往往包括了一些数学计算，而成功与否可能取决于计算是否出现异常或是否满足某些条件。因此，通过返回 bool 类型，可以在计算中传递成功或失败的信息给 Ceres Solver。 ```

struct InequalityConstraint1 {
  template <typename T>
  bool operator()(const T* const x, const T* const y, T* residual) const {
    // 不等式约束 1 <=x^3 + y^3 + xy <=2
    residual[0] = x[0] * x[0] * x[0] + y[0] * y[0] * y[0] + x[0] * y[0] - T(1);
    residual[1] = T(2) - (x[0] * x[0] * x[0] + y[0] * y[0] * y[0] + x[0] * y[0]);
    return true;
  }
};

• 设置不等式约束：

residual[0] = x[0] * x[0] * x[0] + y[0] * y[0] * y[0] + x[0] * y[0] - T(1);
在这里，T(1) 表示一个常数 1，而 T(2) 表示一个常数 2。

关于 residual[0] 的形式，它是不等式约束条件的表示。在这个例子中，不等式约束是：
1 <= x^3 + y^3 + xy <= 2，而 residual[0] 的设置是为了将这个不等式约束转化为优化问题的残差。
具体而言，residual[0] 表达的是不等式的左边 x^3 + y^3 + xy - 1，这样在优化的过程中，通过最小化这个残差，可以使得不等式约束得到满足。
problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CostFunctor, 1, 1, 1>(new CostFunctor),
      nullptr,
      &x, &y
  );

• c++ ceres::AutoDiffCostFunction 是 Ceres Solver 中用于自动求导的类。
  
• C++ <CostFunctor, 1, 1, 1> CostFunctor：指定了代价函数的类型。 1,1,1： 1：表示代价函数的残差维度。在这个例子中，代价函数产生的残差是一个维度为 1 的标量值。 1：表示第一个参数块的大小。在这个例子中，第一个参数块是变量 x，它是一个维度为 1 的标量。 1：表示第二个参数块的大小。在这个例子中，第二个参数块是变量 y，它是一个维度为 1 的标量。
  
• c++ nullptr 默认就行，此处实际默认代价函数f(x)=x;初学者不用管。
  
• c++ &x, &y &x 和 &y 是指向变量 x 和 y 的指针，它们将成为优化的参数
  

ceres::Solver::Options options;

• 在这个结构体中，有很多可以配置的选项，以下是一些常见的选项及其默认值：
  
• linear_solver_type：线性求解器的类型，默认为 ceres::DENSE_QR。
  

这个选项指定了在每次迭代中如何求解线性系统。

1. minimizer_type：最小化器的类型，默认为 ceres::TRUST_REGION.

这个选项指定了用于实现最小化的算法，例如 Levenberg-Marquardt 或 Dogleg。

1. trust_region_strategy_type：信赖域策略的类型，默认为 ceres::LEVENBERG_MARQUARDT。 这个选项定义了在优化中如何动态调整信赖域的策略。
   
2. num_threads：用于求解器的线程数，默认为 1。 指定在求解器中使用的线程数量，以加速优化过程。
   
3. max_num_iterations：最大迭代次数，默认为 50。 指定优化的最大迭代次数，以避免无限迭代。
   
4. parameter_tolerance：参数变化的容忍度，默认为 1e-8。 如果参数的变化小于此容忍度，认为已经收敛。
   
5. function_tolerance：目标函数变化的容忍度，默认为 1e-6。 如果目标函数的变化小于此容忍度，认为已经收敛。
   
6. gradient_tolerance：梯度范数的容忍度，默认为 1e-10。 如果梯度的范数小于此容忍度，认为已经收敛。
   

ceres::Solver::Summary summary;

• ceres::Solver::Summary 是 Ceres Solver 在求解器运行完毕后生成的一个结构体，包含了求解过程的各种信息和统计数据。以下是一些主要的成员和其含义：
• brief_report: 一个字符串，提供了对求解结果的简要描述，包括收敛状态、耗时等信息。
• initial_cost: 初始代价函数值。
• final_cost: 求解器运行结束后的代价函数值。
• iterations: 求解过程中迭代的次数。
• num_successful_steps: 成功步骤的数量。
• num_unsuccessful_steps: 未成功步骤的数量。
• num_inner_iteration_steps: 内部迭代步骤的数量。
• time_in_seconds: 求解器运行的总时间（秒）。
• linear_solver_type_used: 使用的线性求解器类型。
• preconditioner_type_used: 使用的预条件器类型。
• final_gradient_max_norm: 求解结束时的梯度最大范数。
• final_step_norm: 求解结束时的步长范数。

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

1. options: 这是一个 ceres::Solver::Options 类型的对象，其中包含了求解器的各种配置选项。你可以在这里设置迭代次数、终止条件、线性求解器类型等参数，以调整求解过程的行为。
2. &problem: 这是你构建的最小二乘问题的实例，即 ceres::Problem 类型的对象。它包含了代价函数、变量参数等信息，描述了你要优化的目标函数。
3. &summary: 这是一个 ceres::Solver::Summary 类型的对象，用于存储求解的结果摘要信息。在函数调用后，&summary 中将包含有关求解过程的统计信息，例如迭代次数、最终代价函数值、优化参数的最终值等。