目錄
1.Problem類簡述
// 來自於ceres-solver-1.14.0/include/ceres/problem.h
class CERES_EXPORT Problem {
public:
// Problem默認掌握cost_function，loss_function和local_parameterization指針的所有權。這些對象在Problem的整個生命周期都保持活動狀態。
// 如果用戶希望控制這些對象的銷毀行為，那么他們可以通過在Problem :: Options中設置相應的選項來實現。
struct CERES_EXPORT Options {
Options()
: cost_function_ownership(TAKE_OWNERSHIP),
loss_function_ownership(TAKE_OWNERSHIP),
local_parameterization_ownership(TAKE_OWNERSHIP),
enable_fast_removal(false),
disable_all_safety_checks(false),
context(NULL) {}
/*
enum Ownership {
DO_NOT_TAKE_OWNERSHIP,
TAKE_OWNERSHIP
};
*/
/*
默認值: TAKE_OWNERSHIP
作用: 該選項控制Problem對象是否擁有代價函數。
如果設置為TAKE_OWNERSHIP，那么Problem對象將在銷毀時刪除代價函數。析構函數只小心地刪除指針一次，由於允許共享代價函數。
*/
Ownership cost_function_ownership;
/*
默認值: TAKE_OWNERSHIP
作用: 該選項控制Problem對象是否擁有損失函數。
如果設置為TAKE_OWNERSHIP，那么Problem對象將在銷毀時刪除損失函數。析構函數只小心地刪除指針一次，由於允許共享損失函數。
*/
Ownership loss_function_ownership;
/*
默認值: TAKE_OWNERSHIP
作用: 該選項控制Problem對象是否擁有局部參數化。
如果設置為TAKE_OWNERSHIP，那么Problem對象將在銷毀時刪除局部參數化。析構函數只小心地刪除指針一次，由於允許共享局部參數化。
*/
Ownership local_parameterization_ownership;
/*
默認值: false
作用: 如果為true，則用內存交換更快的操作Problem::RemoveResidualBlock()和Problem::RemoveParameterBlock()???
默認情況下，Problem::RemoveParameterBlock()和Problem::RemoveResidualBlock()所花的時間與整個問題的大小成比例。如果您只是偶爾從問題中刪除參數或殘差，這可能是可以接受的。
但是，如果您有空閑的內存，則啟用此選項使Problem::RemoveParameterBlock()占用的時間與依賴它的殘差塊的數量成比例，而Problem::RemoveResidualBlock()占用(平均)常量時間。
*/
bool enable_fast_removal;
/*
默認值: false
作用: 默認情況下，Ceres在構造problem時執行各種安全檢查。這些檢查有一個很小但可測量的性能損失，通常約為構建時間的5%。
如果您確定problem構造是正確的，並且5%的problem構造時間確實是您想要避免的開銷，那么您可以將disable_all_safety_checks設置為true。
WARNING 不要設置為true，除非你絕對確定你在做什么。
*/
bool disable_all_safety_checks;
/*
默認值: nullptr
作用: 用於解決這個Problem的Ceres全局上下文。Ceres沒有獲得指針的所有權。
*/
Context* context;
};
// 默認構造函數等價於調用Problem(Problem::Options())。
Problem();
explicit Problem(const Options& options);
~Problem();
// 添加一個剩余塊到總的成本函數。
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
// 用少量參數添加殘差的方便方法。這是常見的情況。不要將形參塊實參指定為vector，而是將它們列示為指針。
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0, double* x1, double* x2,
double* x3, double* x4, double* x5,
double* x6, double* x7, double* x8,
double* x9);
// 為問題添加一個大小適當的參數塊。具有相同參數的重復調用將被忽略。使用相同的雙指針但大小不同的重復調用將導致未定義行為。
void AddParameterBlock(double* values, int size);
// 向問題添加一個具有適當大小和參數化的參數塊。具有相同參數的重復調用將被忽略。使用相同的雙指針但大小不同的重復調用將導致未定義行為。
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
// 從問題中移除一個參數塊。
void RemoveParameterBlock(double* values);
// 從問題中移除一個殘差塊。
void RemoveResidualBlock(ResidualBlockId residual_block);
// 在優化過程中保持所指示的參數塊為常數。
void SetParameterBlockConstant(double* values);
// 允許指定的參數塊在優化期間變化。
void SetParameterBlockVariable(double* values);
// 如果參數塊被設置為常量，則返回true，否則返回false。
bool IsParameterBlockConstant(double* values) const;
// 為一個參數塊設置局部參數化。
void SetParameterization(double* values,
LocalParameterization* local_parameterization);
// 獲取與此參數塊關聯的局部參數化對象。如果沒有關聯的參數化對象，則返回NULL。
const LocalParameterization* GetParameterization(double* values) const;
// 設置位置為"index"的參數的上下邊界。
void SetParameterLowerBound(double* values, int index, double lower_bound);
void SetParameterUpperBound(double* values, int index, double upper_bound);
// 問題中的參數塊數。始終等於parameter_blocks().size()和parameter_block_sizes().size()。
int NumParameterBlocks() const;
// 通過對所有參數塊的大小求和得到的參數向量的大小。
int NumParameters() const;
// 返回Problem中殘差塊的數量，永遠等於residual_blocks().size()。
int NumResidualBlocks() const;
// 返回殘差向量的大小，包含所有殘差塊內殘差個數的總和。
int NumResiduals() const;
// 參數塊的大小。
int ParameterBlockSize(const double* values) const;
// 參數塊的local parameterization的大小。如果沒有與此參數塊關聯的local parameterization，則ParameterBlockLocalSize = ParameterBlockSize。
int ParameterBlockLocalSize(const double* values) const;
// 給定的參數塊在這個問題中是否存在?
bool HasParameterBlock(const double* values) const;
// 用指向問題中當前參數塊的指針填充傳遞的parameter_blocks vector。在這個調用之后，parameter_block.size() == NumParameterBlocks。
void GetParameterBlocks(std::vector<double*>* parameter_blocks) const;
// 用指向問題中當前殘差塊的指針填充所傳遞的residual_blocks向量。在這個調用之后，residual_blocks.size() == NumResidualBlocks。
void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const;
// 獲取所有依賴於給定殘差塊的參數塊。
void GetParameterBlocksForResidualBlock(
const ResidualBlockId residual_block,
std::vector<double*>* parameter_blocks) const;
// 獲取給定殘差塊的CostFunction。
const CostFunction* GetCostFunctionForResidualBlock(
const ResidualBlockId residual_block) const;
// 獲取給定殘差塊的LossFunction。如果沒有loss函數與這個殘差塊相關聯，則返回NULL。
const LossFunction* GetLossFunctionForResidualBlock(
const ResidualBlockId residual_block) const;
// 獲取所有依賴於給定參數塊的殘差塊。
// 如果Problem::Options::enable_fast_removal為true，那么獲取殘差塊的速度很快，並且只依賴於殘差塊的數量。否則，獲取參數塊的殘差塊將導致對整個Problem對象的掃描。
void GetResidualBlocksForParameterBlock(
const double* values,
std::vector<ResidualBlockId>* residual_blocks) const;
// 控制Problem::Evaluate的選項結構體
struct EvaluateOptions {
EvaluateOptions()
: apply_loss_function(true),
num_threads(1) {
}
/*
需要執行計算的參數塊的集合。這個向量決定了參數塊出現在梯度向量和雅可比矩陣列中的順序。如果parameter_blocks為空，則假定它等於一個包含所有參數塊的向量。
一般來說，在這種情況下，參數塊的順序取決於它們添加到問題中的順序，以及用戶是否刪除了任何參數塊。
注意：這個向量應該包含與用於向問題添加參數塊的指針相同的指針。這些參數塊不應該指向新的內存位置。如果你這么做，就會有壞事發生。
*/
std::vector<double*> parameter_blocks;
/*
需要執行計算的殘差塊的集合。
這個向量決定了殘差發生的順序，以及雅可比矩陣的行是如何排列的。如果residual_blocks為空，則假定它等於包含所有殘差塊的向量。
*/
std::vector<ResidualBlockId> residual_blocks;
// 即使問題中的殘差塊可能包含loss函數，將apply_loss_function設置為false將關閉loss函數在cost函數輸出中的應用。
bool apply_loss_function;
// 要使用的線程數。(需要OpenMP)。
int num_threads;
};
/*
評估問題。任何輸出指針都可以是NULL。使用哪些殘差塊和參數塊由上面的EvaluateOptions結構體控制。
注意1: 計算將使用在構造問題時使用的參數塊指針所指向的內存位置中存儲的值。也就是說,
Problem problem;
double x = 1;
problem.AddResidualBlock(new MyCostFunction, NULL, &x);
double cost = 0.0;
problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
代價在x = 1處計算。如果你想求x = 2處的值，那么
x = 2;
problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
就是這樣做的方法。
注意2: 如果不使用局部參數化，則梯度向量的大小(以及雅可比矩陣中的列數)是所有參數塊的大小之和。
如果一個參數塊具有局部參數化，那么它將為梯度向量(以及雅可比矩陣中的列數)提供“LocalSize”項。
注意3: 當問題正在被解決時，這個函數不能被調用，例如，不能在解決問題期間的迭代結束時從IterationCallback調用它。
*/
bool Evaluate(const EvaluateOptions& options,
double* cost,
std::vector<double>* residuals,
std::vector<double>* gradient,
CRSMatrix* jacobian);
private:
friend class Solver;
friend class Covariance;
// 整個Problem函數內部的核心操作實際上是由類對象內部的 problem_impl_ 操作的，
internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
};
PS:圖片來自https://blog.csdn.net/jdy_lyy/article/details/119360492?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.no_search_link&spm=1001.2101.3001.4242
2.Problem類重要函數
2.1 Problem::AddResidualBlock
向Problem類傳遞殘差模塊的信息。函數原型
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0,...);
參數說明
- cost_function:
代價函數，包含了參數模塊的維度信息。參考《Ceres學習-1.CostFunction》（ https://www.cnblogs.com/vivian187/p/15393995.html ）
- loss_function:
損失函數，用於處理參數中含有野值的情況，避免錯誤量測對估計的影響，常用參數包括HuberLoss、CauchyLoss等（參考Ceres官方文檔 http://ceres-solver.org/nnls_modeling.html#instances ）；該參數可以取NULL或nullptr，此時損失函數為單位函數。
- parameter_blocks:
待優化的參數，可一次性傳入所有參數的指針容器vector<double*>或依次傳入所有參數的指針double*。
一些應用實例
// 例子1: 來自ceres-solver-1.14.0/examples/helloworld.cc
struct CostFunctor {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
double x = 0.5;
// Build the problem.
Problem problem;
// 使用自動求導構建代價函數
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
// 添加殘差
problem.AddResidualBlock(cost_function, NULL, &x);
// 例子2: 來自ceres-solver-1.14.0/examples/powell.cc
struct F1 {
template <typename T> bool operator()(const T* const x1,
const T* const x2,
T* residual) const {
// f1 = x1 + 10 * x2;
residual[0] = x1[0] + 10.0 * x2[0];
return true;
}
};
double x1 = 3.0;
double x2 = -1.0;
Problem problem;
problem.AddResidualBlock(new AutoDiffCostFunction<F1, 1, 1, 1>(new F1),
NULL,
&x1, &x2);
// 例子3: 來自ceres-solver-1.14.0/examples/bundle_adjuster.cc
CostFunction* cost_function;
cost_function =
(FLAGS_use_quaternions)
? SnavelyReprojectionErrorWithQuaternions::Create(
observations[2 * i + 0],
observations[2 * i + 1])
: SnavelyReprojectionError::Create(
observations[2 * i + 0],
observations[2 * i + 1]);
LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
double* camera =
cameras + camera_block_size * bal_problem->camera_index()[i];
double* point = points + point_block_size * bal_problem->point_index()[i];
problem->AddResidualBlock(cost_function, loss_function, camera, point);
2.2 Problem::AddParameterBlock
用戶在調用AddResidualBlock時其實已經隱式地向Problem傳遞了參數模塊，但在一些情況下，需要用戶顯示地向Problem傳入參數模塊（通常出現在需要對優化參數進行重新參數化的情況）。Ceres提供了Problem::AddParameterBlock函數用於用戶顯式傳遞參數模塊。函數原型
void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
其中，第一種函數原型除了會增加一些額外的參數檢查之外，功能上和隱式傳遞參數並沒有太大區別。第二種函數原型則會額外傳入LocalParameterization參數，用於重構優化參數的維數。
查看第3節介紹LocalParameterization
3.LocalParameterization
LocalParameterization類的作用是解決非線性優化中的過參數化問題。所謂過參數化，即待優化參數的實際自由度小於參數本身的自由度。例如在SLAM中，當采用四元數表示位姿時，由於四元數本身的約束（模長為1），實際的自由度為3而非4。此時，若直接傳遞四元數進行優化，冗余的維數會帶來計算資源的浪費，需要使用Ceres預先定義的QuaternionParameterization對優化參數進行重構：
problem.AddParameterBlock(quaternion, 4);// 直接傳遞4維參數
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重構參數，優化時實際使用的是3維的等效旋轉矢量
3.1 Ceres預定義的LocalParameterization
LocalParameterization本身是一個虛基類，詳細定義如下。用戶可以自行定義自己需要使用的子類，或使用Ceres預先定義好的子類。
/* 目的: 有時參數塊x可能會過參數化問題
min f(x)
x
例如，三維中的球體是嵌入在三維空間中的二維流形。
在球面上的每一點上，它的平面切線定義了一個二維切線空間。
對於這個球上定義的代價函數，給定一個點x，在這個點上向球的法線方向移動是沒有用的。
因此，做局部優化的一個更好的方法是在這個點的切空間上優化二維向量delta，然后“移動”到點x + delta，
在這里移動操作涉及到投影回球面。這樣做可以從優化中刪除一個冗余維度，使其在數值上更加健壯和高效。
更一般地，我們可以定義一個函數
x_plus_delta = Plus(x, delta),
其中x_plus_delta的大小與x相同，而delta的大小小於或等於x。函數Plus推廣了向量加法的定義。因此它滿足恆等式
Plus(x, 0) = x, for all x.
一個普通的加號是當delta和x大小相同時
Plus(x, delta) = x + delta
更有趣的情況是，如果x是二維向量，用戶希望保持第一個坐標常量。那么，delta是一個標量，加號定義為
Plus(x, delta) = x + [0] * delta
[1]
一個常見的來自運動的結構問題的例子是，當相機旋轉使用四元數參數化。
只做與定義四元數的4向量正交的更新是有用的。一種方法是讓delta是一個三維向量並定義+為
Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
RHS上兩個4-向量之間的乘法是標准的四元數乘積。
給定g和點x，優化f現在可以重新表述為
min f(Plus(x, delta))
delta
給出這個問題的解delta，則最優值為
x* = Plus(x, delta)
LocalParameterization類定義了函數Plus及其雅可比矩陣，用於計算f關於delta的雅可比矩陣。
*/
class CERES_EXPORT LocalParameterization {
public:
virtual ~LocalParameterization();
/* 加法操作的實現
x_plus_delta = Plus(x, delta)
條件: Plus(x, 0) = x.
*/
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const = 0;
/* The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
jacobian is a row-major GlobalSize() x LocalSize() matrix.
*/
virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
/* local_matrix = global_matrix * jacobian
global_matrix is a num_rows x GlobalSize row major matrix.
local_matrix is a num_rows x LocalSize row major matrix.
jacobian(x) is the matrix returned by ComputeJacobian at x.
這只在GradientProblem中使用。對於大多數正常使用，使用默認實現是可以的。
*/
virtual bool MultiplyByJacobian(const double* x,
const int num_rows,
const double* global_matrix,
double* local_matrix) const;
// Size of x. 參數的實際維數。
// GlobalSize()返回參數塊大小，eg：四元數返回4
virtual int GlobalSize() const = 0;
// Size of delta. 正切空間上的參數維數
// LocalSize()返回參數塊在對應空間的實際大小，eg，四元數返回3
virtual int LocalSize() const = 0;
};
除了LocalParameterization，還有它的一些子類：
- IdentityParameterization
- SubsetParameterization
- QuaternionParameterization
- EigenQuaternionParameterization
- HomogeneousVectorParameterization
- ProductParameterization
舉例介紹QuaternionParameterization
**注意:**
在 ceres 源碼中沒有明確說明之處都認為矩陣 raw memory 存儲方式是 Row Major 的，這與 Eigen 默認的 Col Major 是相反的。
ceres 默認的 Quaternion raw memory 存儲方式是 w, x, y, z，而 Eigen Quaternion 的存儲方式是 x, y, z, w，這就導致在 ceres 代碼中除ceres::QuaternionParameterization 之外還有ceres::EigenQuaternionParameterization。
Eigen Quaternion指的是eigen庫中的函數Eigen::Quaternion(w,x,y,z)函數中，實數w在首；但是實際上它的內部存儲順序是[x y z w]，對其訪問的時候最后一個元素才是w
對三個函數內部存儲順序總結
ceres::QuaternionParameterization：內部存儲順序為(w,x,y,z)
ceres::EigenQuaternionParameterization：內部存儲順序為(x,y,z,w)
Eigen::Quaternion(w,x,y,z)：內部存儲順序為(x,y,z,w)（與構造函數沒有保持一致）
ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法則。
代碼示例
class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
public:
virtual ~QuaternionParameterization() {}
//重載的Plus函數給出了四元數的更新方法，接受參數分別為優化前的四元數【x】，用旋轉矢量表示的增量【delta】，以及更新后的四元數【x_plus_delta】。
//函數首先將增量【delta】由旋轉矢量轉換為四元數，隨后采用標准四元數乘法對四元數進行更新。
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
//GlobalSize 返回值為4，即四元數本身的實際維數。由於在內部優化時，ceres采用的是旋轉矢量，維數為3，因此LocalSize()的返回值為3。
//GlobalSize 就是表示他真正的維數是一個4維的
virtual int GlobalSize() const { return 4; }
//LocalSize是告訴Ceres他表示的東西是一個三維的
virtual int LocalSize() const { return 3; }
};
//=============================================================================
//重載的Plus函數給出了四元數的更新方法，接受參數分別為優化前的四元數【x】，用旋轉矢量表示的增量【delta】，以及更新后的四元數【x_plus_delta】。
//函數首先將增量【delta】由旋轉矢量轉換為四元數，隨后采用標准四元數乘法對四元數進行更新。
bool QuaternionParameterization::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
// 將旋轉矢量轉換為四元數形式
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
double q_delta[4];
q_delta[0] = cos(norm_delta);
q_delta[1] = sin_delta_by_delta * delta[0];
q_delta[2] = sin_delta_by_delta * delta[1];
q_delta[3] = sin_delta_by_delta * delta[2];
// 采用四元數乘法更新
QuaternionProduct(q_delta, x, x_plus_delta);
} else {
for (int i = 0; i < 4; ++i) {
x_plus_delta[i] = x[i];
}
}
return true;
}
TODO
Plus和ComputeJacobian參考
https://blog.csdn.net/jdy_lyy/article/details/119360492?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_title~default-0.no_search_link&spm=1001.2101.3001.4242
https://blog.csdn.net/hjwang1/article/details/107869743
https://blog.csdn.net/weixin_43991178/article/details/100532618
3.2 自定義LocalParameterization
用戶自定義一個類繼承LocalParameterization類，主要實現的有
Plus(const double* x,const double* delta,double* x_plus_delta)：定義變量的加法
ComputeJacobian()：x對delta的雅克比矩陣
GlobalSize()：x 的自由度（可能有冗余），比如四元數的自由度是4
LocalSize()：Δx 所在的正切空間（tangent space）的自由度，那么這個自由度是3
可以參考上面的QuaternionParameterization類
參考
https://blog.csdn.net/jdy_lyy/article/details/119360492?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.no_search_link&spm=1001.2101.3001.4242
https://blog.csdn.net/weixin_43991178/article/details/100532618
https://blog.csdn.net/hjwang1/article/details/107869743