一、ceressolver自定义雅克比矩阵
Ceres Solver是一个C++开源库，主要用于非线性最小二乘问题的求解。在使用Ceres Solver的过程中，有时需要对雅克比矩阵进行自定义。对于一些单纯形约束的问题，雅克比矩阵的计算比较简单，可以直接利用Ceres Solver提供的ceres::NumericDiffCostFunction实现。但对于复杂的问题，在自定义雅克比矩阵时需要格外小心。
下面通过一个简单的例子介绍如何自定义雅克比矩阵：
class MyCostFunction : public ceres::CostFunction {
public:
virtual bool Evaluate(double const* const* parameters, double* residuals,
double** jacobians) const {
const double a = parameters[0][0];
const double b = parameters[1][0];
residuals[0] = 10.0 - a * b;
if (jacobians != nullptr) {
jacobians[0][0] = -b;
jacobians[0][1] = -a;
jacobians[1][0] = 0.0;
jacobians[1][1] = -10.0 / b;
}
return true;
}
virtual int NumResiduals() const { return 1; }
virtual int NumParameters() const { return 2; }
};
int main(int argc, char** argv) {
double a = 1.0;
double b = 1.0;
ceres::Problem problem;
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction(new MyCostFunction);
problem.AddResidualBlock(cost_function, nullptr, &a, &b);
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);
std::cout << summary.FullReport() << std::endl;
return 0;
}
在上述代码中，MyCostFunction是定义的代价函数，实现的是一个非线性最小二乘问题。在CostFunction::Evaluate函数中，分别计算了残差和雅克比矩阵。其中，parameters是参数向量(a, b)，residuals是残差向量，jacobians是雅克比矩阵的指针数组。在调用Solve函数进行求解时，添加了一个ResidualBlock，即一个代价函数关于一组参数的计算结果。
二、ceres solver 动态库
使用Ceres Solver时，可以选择使用动态库或者静态库。相比静态库，动态库可以在构建时就将其所依赖库的链接信息解析完成，而不需要等到程序运行时再去解析。由于Ceres Solver实现的复杂性，建议使用动态库。
下面介绍如何构建和使用ceres solver动态库：
1、安装必要的依赖包
需要安装cmake， GLOG，GFLAGS，ATLAS，Eigen，SuiteSparse和CXSparse等依赖包。
2、下载ceres solver源码
可以从Ceres Solver官网下载最新源码：http://ceres-solver.org。
3、构建动态库
# run from the top-level directory of the ceres source tree
mkdir ceres-bin
cd ceres-bin
cmake .. -DBUILD_SHARED_LIBS=ON
make -j3
make test
make install
4、使用动态库
编写代码时，在Makefile或CMakeLists.txt中添加链接选项-lceres即可使用。
三、ceres solver se3
在使用Ceres Solver维护机器人的位姿时，需要采用四元数或李代数以尽可能避免将位姿参数化为欧拉角。在机器人路径规划等应用中，需要在两个位姿中使用四元数或李代数进行插值。Ceres Solver库提供了几个方便的工具类来帮助处理这些问题，其中就包括SE3工具类。
下面通过一个简单的例子介绍如何使用ceres solver se3工具类：
#include "ceres/rotation.h"
#include "ceres/types.h"
#include "ceres/local_parameterization.h"
struct Pose {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
double t[3];
double q[4];
};
class PoseParameterization : public ceres::LocalParameterization {
public:
PoseParameterization() {}
virtual ~PoseParameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
Eigen::Map q(x + 6);
Eigen::Map
t(x);
Eigen::Map
q_plus_delta(x_plus_delta + 6);
Eigen::Map
t_plus_delta(x_plus_delta);
t_plus_delta = t + Eigen::Map
(delta);
q_plus_delta = (Eigen::Quaterniond(
Eigen::AngleAxisd(delta[3], Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(delta[4], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(delta[5], Eigen::Vector3d::UnitZ())) *
q).normalized();
return true;
}
virtual bool ComputeJacobian(const double* x, double* jacobian) const {
Eigen::Map
> j(jacobian); j.setZero(); j.topLeftCorner(6, 6).setIdentity(); return true; } virtual int GlobalSize() const { return 7; } virtual int LocalSize() const { return 6; } }; int main(int argc, char** argv) { Pose pose1; pose1.t[0] = 0.1; pose1.t[1] = 0.2; pose1.t[2] = 0.5; double angle = 0.4; pose1.q[0] = std::cos(angle / 2.0); pose1.q[1] = std::sin(angle / 2.0); pose1.q[2] = 0.0; pose1.q[3] = 0.0; Pose pose2; pose2.t[0] = 0.3; pose2.t[1] = 0.1; pose2.t[2] = -0.1; angle = -0.6; pose2.q[0] = std::cos(angle / 2.0); pose2.q[1] = std::sin(angle / 2.0); pose2.q[2] = 0.0; pose2.q[3] = 0.0; double params[14] = { pose2.t[0], pose2.t[1], pose2.t[2], pose2.q[0], pose2.q[1], pose2.q[2], pose2.q[3], pose1.t[0], pose1.t[1], pose1.t[2], pose1.q[0], pose1.q[1], pose1.q[2], pose1.q[3] }; ceres::Problem problem; ceres::LocalParameterization* pose_parameterization = new PoseParameterization; problem.AddParameterBlock(params, 7, pose_parameterization); Eigen::Quaterniond q(pose1.q[0], pose1.q[1], pose1.q[2], pose1.q[3]); Eigen::Vector3d t(pose1.t[0], pose1.t[1], pose1.t[2]); Eigen::Isometry3d pose1_t(q); pose1_t.pretranslate(t); q = Eigen::Quaterniond(pose2.q[0], pose2.q[1], pose2.q[2], pose2.q[3]); t = Eigen::Vector3d(pose2.t[0], pose2.t[1], pose2.t[2]); Eigen::Isometry3d pose2_t(q); pose2_t.pretranslate(t); ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction
( new SE3CostFunctor(pose1_t.inverse() * pose2_t)); problem.AddResidualBlock(cost_function, nullptr, params); 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); Eigen::Quaterniond q_final(params[3], params[4], params[5], params[6]); Eigen::Vector3d t_final(params[0], params[1], params[2]); Eigen::Isometry3d pose_final_q(q_final); pose_final_q.pretranslate(t_final); std::cout << pose_final_q.matrix() << std::endl; return 0; }
上述代码中，首先定义了一个Pose结构体来存储位姿信息。然后编写了一个PoseParameterization类来实现SE3的本地参数化，并将其传递给ceres::Problem。接下来，使用Eigen库创建两个位姿变换对象pose1_t和pose2_t，并将pose1_t作为参考系，计算相对位姿关系。添加残差时，将目标位姿关系与当前参数进行比较。在Solve函数中，给出了求解选项和求解结果。
四、ceres solver arm
为了在嵌入式设备上使用Ceres Solver，需要将其移植到所使用的平台上。在ARM架构的平台上移植Ceres Solver需要解决许多问题，如编译器支持、内存管理、线程调度等。以下是一些Tips可供参考：
1、选择适合嵌入式系统的编译器
在ARM架构上，推荐使用GCC编译器，因为它是免费的。GCC提供了对ARM平台的完全支持，同时还支持许多针对ARM平台的扩展和选项。
2、优化编译选项
在编译时，需要使用-Ofast等优化选项，以最大程度地提高代码效率。
3、内存管理
在嵌入式系统上，通常内存是有限的。需要有效地管理内存以确保Ceres Solver的高效运行。可以通过实现定制的内存池来做到这一点。
4、线程调度
在使用多线程进行优化时，需要使用线程池等技术来减少同步开销，同时还可以通过设置不同线程的优先级来降低线程调度的负担。
五、ceres solver安装
Ceres Solver的安装方法可参考第二节ceres solver动态库。
六、ceres solver neon
NEON是在ARM处理器上广泛使用的SIMD指令集，可以显著提高代码效率。Ceres Solver通过使用NEON指令集来加速矩阵运算。在使用NEON加速时，需要选择正确的数据结构，尽量将矩阵数据结构拆分为4个单独的浮点数数组。
以下是一个使用NEON加速的矩阵乘法例子：
#include "ceres/neon_intrinsics.h"
void MultiplyMatrixUsingNEON(const float* A, const int A_rows,
const int A_cols, const float* B,
const int B_cols, float* C) {
assert(A_cols % 4 == 0 && B_cols % 4 == 0);
const int C_rows = A_rows;
