一、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<Eigen::Matrix >
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;原創文章，作者：小藍，如若轉載，請註明出處：https://www.506064.com/zh-tw/n/230259.html
微信掃一掃 
支付寶掃一掃 