1.Ceres中求解一個優化問題的結構
背景：在SLAM中，很多問題都是在求解Translation（包含旋轉和平移量），因此這裏以其爲代表，來分析使用ceres如何對其近求導。
void Calibrator::Optimize(Eigen::Matrix4d& tf)
{
//待優化參數分別爲rotation和t
Eigen::Matrix3d rot = T_.topLeftCorner(3,3);
Eigen::Quaterniond q (rot);
Eigen::Vector3d p = T_.topRightCorner(3,1);
ceres::Problem problem;
ceres::Solver::Summary summary;
ceres::Solver::Options options;
ceres::LossFunction* loss_function_edge (new ceres::SoftLOneLoss(1));
ceres::LocalParameterization* quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
for(const auto& ply : polygons_)
{
// add edge 誤差來源：每一條3D點雲邊上的點，到2D平面的直線的距離
for(uint32_t i=0; i<size_; i++)
{
if(ply.pc->edges[i].points.cols() == 0)
{
continue;
}
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));//
problem.AddResidualBlock(cost, loss_function_edge,q.coeffs().data(), p.data());
}
}
problem.SetParameterization(q.coeffs().data(), quaternion_local_parameterization);
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5000;
options.num_threads= boost::thread::hardware_concurrency() - 1;
options.num_linear_solver_threads = options.num_threads;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
//打印最後的優化結果
tf = Eigen::Matrix4d::Identity();
tf.topLeftCorner(3,3) = q.matrix();
tf.topRightCorner(3,1) = p;
std::cout << "T: \n" << tf << std::endl;
}
1. ceres::Solver::Summary summary
優化中的產生的各種信息。
2. ceres::Solver::Options options;
對優化器進行配置。例如使用多少個核來進行計算，最大迭代次數等。
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5000;
// or use all cpu cores
options.num_threads= boost::thread::hardware_concurrency() - 1;
options.num_linear_solver_threads = options.num_threads;
3. ceres::Solve(options, &problem, &summary)
ceres::Solve(options, &problem, &summary);
4. ceres::Problem
最後則是如何構建整個優化問題。這裏需要詳細分析。
ceres::Problem problem;
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));
//添加殘差塊
problem.AddResidualBlock(cost, loss_function_edge,
q.coeffs().data(), p.data() );
// add inlier points 添加內點
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Point2PolygonError,1,4,3>(
new Point2PolygonError(K_, ply.pc->inliers, ply.img->vertexs) );
problem.AddResidualBlock(cost, NULL, q.coeffs().data(), p.data());
problem.SetParameterization(q.coeffs().data(), quaternion_local_parameterization);
4.1 首先是cost function的構建
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));
那麼，cost function的詳細定義如下：
/**
* @brief Edge to edge error (point to line error)
* 3D point P_i_j locates at 3D edge i, its index is j. 3D edge i has correspondense in
* image E_i(ax+by+c=0,a^2+b^2+c^2=1), project P_i_j into image with initial T(q,p) and
* get 2D point P'_i_j(u,v). If T(q,p) is correct or precise, P'_i_j should be on E_i.
* Thus, the error is the distance between P'_i_j and E_i
*/
struct Edge2EdgeError
{
const Eigen::Matrix3d& K; // camera matrix
const Eigen::Matrix3Xd& pts; // 3d points
const Eigen::Vector3d& coef; // 2d edge(line) coefficients
/**
* @brief Edge2EdgeError constructor 輸入只有內參，3D點，和2D的直線方程。
*
* @param k [in]: camera intrinsic parameter matrix K
* @param ps[in]: 3D edge pints set, P_i_j(j=0,1,...n), it's 3xn matrix
* @param cf[in]: 2D line coefficients (ax+by+c=0,a^2+b^2+c^2=1)
*/
Edge2EdgeError(const Eigen::Matrix3d& k, const Eigen::Matrix3Xd& ps, const Eigen::Vector3d& cf):
K(k), pts(ps), coef(cf) {}
/**
* @brief Ceres error compute function
*
* @param T double or Jet
* @param q [in]: Quaternion (rotation)
* @param p [in]: translation
* @param residuals[out]: error
*
* @return true: indicate success
*/
template<typename T>
bool operator()(const T* const q, const T* const p, T* residuals)const
{
Eigen::Matrix<T, 3, Eigen::Dynamic> points;
ProjectPoint2Image<T>(q,p, K.cast<T>(), pts.cast<T>(), points);
//輸出的points是圖像平面的座標點
//points:3*n n是點的數量
//coef:3*1 是abc三個參數
//所以coef.transpose()*points = 1*3*3*n = 1*n
//
Eigen::Matrix<T, Eigen::Dynamic, 1> tmp = (coef.transpose().cast<T>())*points;
residuals[0] = tmp.cwiseAbs().sum(); //算出來有正有負，這個cwiseAbs是求絕對值。然後加和
return true; // important
}
};
這裏面需要注意的主要有三點：
- 第零點：自動求導的定義：
new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(new Edge2EdgeError(T1，T2，....));
這個地方比較重要的地方有兩個；
①自動求導的構造函數輸入，就是自定義的cost function的實例的指針。這個具體在下一點講。
②模板參數<Edge2EdgeError, 1,4,3>:
第一個是cost function。
第二個是殘差的維度
第三個是待優化參數的維度。這裏是4，表示旋轉的四元數表示。
第四個也是待優化參數的維度。這裏是3，表示平移量的xyz。
也可以有56789個，ceres內部根據傳入參數的數量進行了重載。
但是，不管有多少個待優化的參數，這裏的4,3的順序是和後面的AddResidualBlock()的輸入對應的！
- 第一點：構造函數的定義。也就是下面這行需要注意的：
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef )
在實例化這一個cost function的時候，把就按殘差所需要的數據進行了傳入。
-
第二點：實現對operator()的重載構造仿函數。
在對符號進行重載的函數裏，實現了對殘差的計算。
4.2 對整個問題也就是problem添加AddResidualBlock();也即如下：
problem.AddResidualBlock(cost, loss_function_edge,q.coeffs().data(), p.data());
注意，這裏的AddResidualBlock(T1，T2，T3，T4)一共輸入了四個參數。
其中，T1是cost的
T2是損失函數，也即是核函數。
T3是前面所強調的四元數
T4是平移量。
需要注意的是，這裏的待優化參數的傳入是變量的地址！
2.求解器的分類
上面所舉的例子的分析是使用的自動求導。實際上ceres還有其他的求導方式。他們分別是：
1.自動求導（Automatic Differentiation）
2.解析求導（Analytic Differentiation）
3.數值求導（numeric Differentiation）
下面我們分別對這三種求導方式進行對比分析
2.1 自動求導（Automatic Differentiation）
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);
2.2 數值求導（numeric Differentiation）
struct NumericDiffCostFunctor {
bool operator()(const double* const x, double* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
CostFunction* cost_function = new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
注意，和自動求導相比，唯一的區別在於模板參數裏多了一個ceres::CENTRAL
所以，在ceres的官方文檔裏對這兩種求導方法進行了對比：
Generally speaking we recommend automatic differentiation instead of numeric differentiation. The use of C++ templates makes automatic differentiation efficient, whereas numeric differentiation is expensive, prone to numeric errors, and leads to slower convergence.
簡單來說，就是自動求導又快又好。數值求導又慢又不穩定。
2.3 解析求導（Analytic Differentiation）
但是不是什麼情況下都是可以使用自動求導的。在某些情況下，使用閉式求解會更快更有效率，而不是拘泥於自動求導的鏈式求導規則。
//Minimize 0.5 (10 - x)^2 using analytic jacobian matrix.
class QuadraticCostFunction
: public SizedCostFunction<1 /* number of residuals */,
1 /* size of first parameter */> {
public:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x = parameters[0][0];
// f(x) = 10 - x.
residuals[0] = 10 - x;
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1;
}
return true;
}
};
//////
// Set up the only cost function (also known as residual).
CostFunction* cost_function = new QuadraticCostFunction;
problem.AddResidualBlock(cost_function, NULL, &x);
注意，解析求導和之前兩個不一樣的地方有兩點：
1.最後CostFunction* 指向的直接就是自己定義的殘差函數
2.自己定義的殘差函數需要繼承ceres自己定義的：
SizedCostFunction<1 /* number of residuals */,
1 /* size of first parameter */> {
這裏面的模板參數定義了殘差和待優化變量的維度。
同時沒有了之前的仿函數，轉而對Evaluate進行了繼承和重載。
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x = parameters[0][0];
// f(x) = 10 - x.
residuals[0] = 10 - x;
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1;
}
return true;
}
輸入的參數有
double const* const* parameters：輸入參數二維矩陣。
double* residuals：殘差，一維向量
double** jacobians：雅克比矩陣，二維矩陣
這裏的輸入的參數的維度該如何理解呢？爲什麼直接parameters就是二維矩陣了呢？
留待下文分析！