這篇文章就簡(jiǎn)單講一個(gè)例子，希望大家動(dòng)手做一個(gè)demo，這樣來(lái)體會(huì)這個(gè)庫(kù)的使用方式。
理論部分
不知道大家還記不記得g2o的經(jīng)典demo，sphere.我們今天就來(lái)自己用ceres做一個(gè)圖優(yōu)化，從而學(xué)會(huì)ceres的使用。李群李代數(shù)基本功我不想在這里太多的講解，就給大家簡(jiǎn)單的把公式推導(dǎo)一下，然后上代碼(這個(gè)誤差定義未必好，想做的更好的朋友可以考慮用更好的誤差定義，從而有更好的線性化策略)，廢話不多說(shuō)，直接上公式，為了簡(jiǎn)略起見(jiàn)，我就不去寫那個(gè)hat和vee符號(hào)了，相信大多數(shù)朋友對(duì)這套玩意兒早已爛熟于心
如上圖，最后就這樣得到了線性化結(jié)果，然后你就可以拿去定義g2o的邊了，不過(guò)這不是今天的重點(diǎn)，接下來(lái)我們來(lái)用ceres寫圖優(yōu)化
ceres-solver使用
這個(gè)程序我已經(jīng)上傳到我的github上，沒(méi)怎么去考慮程序設(shè)計(jì)，這個(gè)程序附帶顯示端，私以為做個(gè)仿真應(yīng)該還是可以了，記得給一個(gè)星星哦，網(wǎng)址為:https://github.com/Lancelot899/g2o_sphere
同時(shí)還有一個(gè)ceres寫得pnp的仿真，程序和這個(gè)程序也類似，只有雅可比計(jì)算有一些不同，大家有興趣也可以看看https://github.com/Lancelot899/ceres_pnp
廢話不多說(shuō)，進(jìn)入程序編寫部分，首先大家可以在我的github上把這個(gè)程序下載下來(lái)，外圍的數(shù)據(jù)讀取顯示等都已完成，優(yōu)化的核心代碼在Sphere.cpp文件中，首先我先介紹一下需要打交道的數(shù)據(jù)結(jié)構(gòu)，這部分代碼在DataStruct.h文件中
struct Vertex {
int index;
Eigen::Matrix<double, 6, 1> pose;
};
struct Edge {
int i;
int j;
Sophus::SE3d pose;
Eigen::Matrix<double, 6, 6> infomation;
};
點(diǎn)數(shù)據(jù)結(jié)構(gòu)里面有點(diǎn)的索引，以及表示位姿的李代數(shù)的值，邊數(shù)據(jù)結(jié)構(gòu)存儲(chǔ)了邊連接的點(diǎn)的索引，以及邊的約束(SE3)，還有信息矩陣，這些數(shù)據(jù)的讀取和顯示已經(jīng)在程序的其他部分做好了
然后是Sphere.cpp文件，在這個(gè)文件中，大家可以重新定義誤差，然后實(shí)現(xiàn)一個(gè)自己的優(yōu)化。使用ceres做優(yōu)化，首先要做的是定義損失函數(shù)，在官方網(wǎng)站上有很多可以定義損失函數(shù)的方法，在這里我選擇了一個(gè)相對(duì)比較靈活的方式，即繼承"ceres::SizedCostFunction"
這是個(gè)可變參數(shù)的模板，第一個(gè)參數(shù)為誤差的維度，由于誤差是SE(3)的李代數(shù)se(3)，所以是6維，后面的參數(shù)都是你要傳入的參數(shù)的維度，這里要優(yōu)化兩個(gè)位姿，所以需要傳入兩個(gè)參數(shù)，每個(gè)位姿的維度為6，所以設(shè)置兩個(gè)6，因此在程序中繼承它的時(shí)候，傳入了模板參數(shù)<6,6,6>，之后核心在于重載函數(shù)
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const;
注意這里是的const，一個(gè)不能少，我來(lái)介紹一下這個(gè)函數(shù)怎么寫。
由于你有兩個(gè)傳入?yún)?shù)，所以parameters和jacobians這個(gè)二級(jí)指針里面，含有兩個(gè)一級(jí)指針，就是說(shuō)parameters[0]和parameters1,以及jacobians[0]和jacobians1這四個(gè)指針是有內(nèi)存的，當(dāng)然residuals也是有內(nèi)存的
之后，由于param的維度都是6，所以parameters[0]和parameters1的空間分配量都為6,residuals自然也是6，注意，jacobians[0]和jacobians1兩個(gè)雅可比指針的維度都是36,因?yàn)檎`差是6維,參數(shù)是6維,求導(dǎo)得到的矩陣為6*6，在這里把這個(gè)矩陣存成向量，行優(yōu)先存儲(chǔ)。你需要在這個(gè)函數(shù)里面把殘差已經(jīng)雅可比全部計(jì)算好，然后返回一個(gè)true
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie_i(*parameters);
Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie_j(*(parameters + 1));
Eigen::Matrix<double, 6, 6> Jac_i;
Eigen::Matrix<double, 6, 6> Jac_j;
Sophus::SE3d T_i = Sophus::SE3d::exp(lie_i);
Sophus::SE3d T_j = Sophus::SE3d::exp(lie_j);
Sophus::SE3d Tij_estimate = T_i * T_j.inverse();
Sophus::SE3d err = Tij_estimate * T_ij_.inverse();
Eigen::Matrix<double, 6, 6> Jl;
Jl.block(3, 3, 3, 3) = Jl.block(0, 0, 3, 3) = Sophus::SO3d::hat(err.so3().log());
Jl.block(0, 3, 3, 3) = Sophus::SO3d::hat(err.translation());
Jl.block(3, 0, 3, 3) = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 6, 6> I = Eigen::Matrix<double, 6, 6>::Identity();
Jl.noalias() = sqrt_information_ * (I - 0.5 * Jl);
Jac_i = Jl;
Eigen::Matrix<double, 6, 1> err_ = sqrt_information_ * err.log();
const Eigen::Matrix<double, 3, 3>& R = Tij_estimate.rotationMatrix();
Eigen::Matrix<double, 6, 6> adj;
adj.block<3, 3>(3, 3) = adj.block<3, 3>(0, 0) = R;
adj.block<3, 3>(0, 3) = Sophus::SO3d::hat(Tij_estimate.translation()) * R;
adj.block<3, 3>(3, 0) = Eigen::Matrix<double, 3, 3>::Zero(3, 3);
Jac_j = -Jac_i * adj;
int k = 0;
for(int i = 0; i < 6; i++) {
residuals[i] = err_(i);
if(jacobians) {
for (int j = 0; j < 6; ++j) {
if (jacobians[0])
jacobians[0][k] = Jac_i(i, j);
if (jacobians[1])
jacobians[1][k] = Jac_j(i, j);
k++;
}
}
}
return true;
}
這段代碼應(yīng)該能和上面的公式都對(duì)上，應(yīng)該說(shuō)明的兩個(gè)地方為sqrt_information_，由于整個(gè)誤差計(jì)算是有信息矩陣的，你需要把信息矩陣分解一下，然后乘到雅可比上，最后得到不帶權(quán)值的最小二乘法，簡(jiǎn)單用公式說(shuō)明就是:
所以大家可以看到我在構(gòu)造函數(shù)里面把這個(gè)事做了，然后后面直接乘一下雅可比得到新的最小二乘法。
Eigen::LLT<Eigen::Matrix<double, 6, 6>> llt(information);
sqrt_information_ = llt.matrixL();
之后，由于大家都知道，點(diǎn)里面的位姿是李代數(shù)，所以更新的時(shí)候是不能夠線性相加的，而要用這樣那樣的公式：
因此你需要自己設(shè)置更新方法，這個(gè)就是使用本地化參數(shù)了，需要繼承" ceres::LocalParameterization"這個(gè)類，實(shí)現(xiàn)四個(gè)接口，分別是:
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x,
double* jacobian) const;
virtual int GlobalSize() const;
virtual int LocalSize() const ;
在我的程序里面，雅可比都在前面全部搞定了，也沒(méi)有本地參數(shù)(你實(shí)際上要用的)和全局參數(shù)(最小二乘法算的)參數(shù)的變化，所以你會(huì)看到ComputeJacobian這個(gè)函數(shù)，我就設(shè)置了個(gè)單位矩陣上去，然后實(shí)現(xiàn)了Plus這個(gè)函數(shù)
做好這些事，你的準(zhǔn)備工作就算全部寫好了，最后就是設(shè)置求解器,都是套路活兒，代碼是死的，大家看看我寫的應(yīng)該就會(huì)了，就寫這么多，有什么問(wèn)題，可以私下與我聯(lián)系
