优化库——ceres(三)实战案例
实战案例
1.1 CmakeLists.txt配置
cmake_minimum_required(VERSION 2.8)
project(ceres)
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
add_executable(test test.cpp)
target_link_libraries(test ${CERES_LIBRARIES})
1.2 示例:ceres入门例子
一个简单的求解 min x ( 10 − x ) 2 min _{x} (10-x)^{2} minx(10−x)2 的优化问题代码如下:
#include<iostream>
#include<ceres/ceres.h>
using namespace std;
using namespace ceres;
//第一部分:构建代价函数,重载()符号,仿函数的小技巧
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
//主函数
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// 寻优参数x的初始值,为5
double initial_x = 5.0;
double x = initial_x;
// 第二部分:构建寻优问题
Problem problem;
//使用自动求导,将之前的代价函数结构体传入,第一个1是输出维度,即残差的维度,第二个1是输入维度,即待寻优参数x的维度。
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
//向问题中添加误差项,本问题比较简单,添加一个就行。
problem.AddResidualBlock(cost_function, NULL, &x);
//第三部分: 配置并运行求解器
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
options.minimizer_progress_to_stdout = true;//输出到cout
Solver::Summary summary;//优化信息
Solve(options, &problem, &summary);//求解!!!
std::cout << summary.BriefReport() << "
";//输出优化的简要信息
//最终结果
std::cout << "x : " << initial_x << " -> " << x << "
";
return 0;
}
1.3 应用:曲线拟合(使用的是自动求导,不用写雅克比)
以下内容来源与参考:
问题:拟合非线性函数的曲线
y = e 3 x 2 + 2 x + 1 y=e^{3 x^{2}+2 x+1} y=e3x2+2x+1
整个代码的思路还是先构建代价函数结构体,然后在[0,1]之间均匀生成待拟合曲线的1000个数据点,加上方差为1的白噪声,数据点用两个vector储存(x_data和y_data),然后构建待求解优化问题,最后求解,拟合曲线参数。
(PS. 本段代码中使用OpenCV的随机数产生器,要跑代码的同学可能要先装一下OpenCV)
#include<iostream>
#include<opencv2/core/core.hpp>
#include<ceres/ceres.h>
using namespace std;
using namespace cv;
//构建代价函数结构体,abc为待优化参数,residual为残差。
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
template <typename T>
bool operator()(const T* const abc,T* residual)const
{
residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
return true;
}
const double _x,_y;
};
//主函数
int main()
{
//参数初始化设置,abc初始化为0,白噪声方差为1(使用OpenCV的随机数产生器)。
double a=3,b=2,c=1;
double w=1;
RNG rng;
double abc[3]={0,0,0};
//生成待拟合曲线的数据散点,储存在Vector里,x_data,y_data。
vector<double> x_data,y_data;
for(int i=0;i<1000;i++)
{
double x=i/1000.0;
x_data.push_back(x);
y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w));
}
//反复使用AddResidualBlock方法(逐个散点,反复1000次)
//将每个点的残差累计求和构建最小二乘优化式
//不使用核函数,待优化参数是abc
ceres::Problem problem;
for(int i=0;i<1000;i++)
{
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
new CURVE_FITTING_COST(x_data[i],y_data[i])
),
nullptr,
abc
);
}
//配置求解器并求解,输出结果
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);
cout<<"a= "<<abc[0]<<endl;
cout<<"b= "<<abc[1]<<endl;
cout<<"c= "<<abc[2]<<endl;
return 0;
}
}
1.4 应用: 基于李代数的视觉SLAM位姿优化(解析求导)
以下内容来源与参考:
下面以 基于李代数的视觉SLAM位姿优化问题 为例,介绍 Ceres Solver 的使用。
(1)残差(预测值 - 观测值)
r
(
ξ
)
=
K
exp
(
ξ
∧
)
P
−
u
r(xi)=K exp left(xi^{wedge}
ight) P-u
r(ξ)=Kexp(ξ∧)P−u
(2)雅克比矩阵
J
=
∂
r
(
ξ
)
∂
ξ
=
[
f
x
Z
′
0
−
X
′
f
x
Z
′
2
−
X
′
Y
′
f
x
Z
′
2
f
x
+
X
′
2
f
x
Z
′
2
−
Y
′
f
x
Z
′
0
f
y
Z
′
−
Y
′
f
y
Z
2
−
f
y
−
Y
′
2
f
y
Z
′
2
X
′
Y
′
f
y
Z
′
2
X
′
f
y
Z
′
]
∈
R
2
×
6
J=frac{partial r(xi)}{partial xi} \ =left[egin{array}{ccccc} frac{f_{x}}{Z^{prime}} & 0 & -frac{X^{prime} f_{x}}{Z^{prime 2}} & -frac{X^{prime} Y^{prime} f_{x}}{Z^{prime 2}} & f_{x}+frac{X^{prime 2} f_{x}}{Z^{prime 2}} & -frac{Y^{prime} f_{x}}{Z^{prime}} \ 0 & frac{f_{y}}{Z^{prime}} & -frac{Y^{prime} f_{y}}{Z^{2}} & -f_{y}-frac{Y^{prime 2} f_{y}}{Z^{prime 2}} & frac{X^{prime} Y^{prime} f_{y}}{Z^{prime 2}} & frac{X^{prime} f_{y}}{Z^{prime}} end{array}
ight] in mathbb{R}^{2 imes 6}
J=∂ξ∂r(ξ)=[Z′fx00Z′fy−Z′2X′fx−Z2Y′fy−Z′2X′Y′fx−fy−Z′2Y′2fyfx+Z′2X′2fxZ′2X′Y′fy−Z′Y′fxZ′X′fy]∈R2×6
(3)核心代码
代价函数的构造:
//应该是重投影误差,这样的话才会有残差2维的,优化变量是6维度的
class BAGNCostFunctor : public ceres::SizedCostFunction<2, 6> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //对齐
//? 二维,三维? 第一个是像素观测值 第二个是准备用来重投影的三维观测值
BAGNCostFunctor(Eigen::Vector2d observed_p, Eigen::Vector3d observed_P) :
observed_p_(observed_p), observed_P_(observed_P) {}
virtual ~BAGNCostFunctor() {}
virtual bool Evaluate(
//参数 残差 雅克比
double const* const* parameters, double *residuals, double **jacobians) const {
Eigen::Map<const Eigen::Matrix<double,6,1>> T_se3(*parameters);
Sophus::SE3 T_SE3 = Sophus::SE3::exp(T_se3);
Eigen::Vector3d Pc = T_SE3 * observed_P_;
Eigen::Matrix3d K;
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
K << fx, 0, cx, 0, fy, cy, 0, 0, 1;
//! 计算残差
Eigen::Vector2d residual = observed_p_ - (K * Pc).hnormalized();
residuals[0] = residual[0];
residuals[1] = residual[1];
if(jacobians != NULL) {
if(jacobians[0] != NULL) {
// 2*6的雅克比矩阵
Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> J(jacobians[0]);
double x = Pc[0];
double y = Pc[1];
double z = Pc[2];
double x2 = x*x;
double y2 = y*y;
double z2 = z*z;
J(0,0) = fx/z;
J(0,1) = 0;
J(0,2) = -fx*x/z2;
J(0,3) = -fx*x*y/z2;
J(0,4) = fx+fx*x2/z2;
J(0,5) = -fx*y/z;
J(1,0) = 0;
J(1,1) = fy/z;
J(1,2) = -fy*y/z2;
J(1,3) = -fy-fy*y2/z2;
J(1,4) = fy*x*y/z2;
J(1,5) = fy*x/z;
}
}
return true;
}
private:
const Eigen::Vector2d observed_p_;
const Eigen::Vector3d observed_P_;
};
构造优化问题,并求解相机位姿:
Sophus::Vector6d se3;
//在当前problem中添加代价函数残差块,损失函数为NULL采用默认的最小二乘误差即L2范数,优化变量为 se3
ceres::Problem problem;
for(int i=0; i<n_points; ++i) {
ceres::CostFunction *cost_function;
cost_function = new BAGNCostFunctor(p2d[i], p3d[i]);
problem.AddResidualBlock(cost_function, NULL, se3.data());
}
ceres::Solver::Options options;
options.dynamic_sparsity = true;
options.max_num_iterations = 100; //迭代100次
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = true;
options.dogleg_type = ceres::SUBSPACE_DOGLEG;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "
";
std::cout << "estimated pose:
" << Sophus::SE3::exp(se3).matrix() << std::endl;
相关文章
- 【技术种草】cdn+轻量服务器+hugo=让博客“云原生”一下
- CLB运维&运营最佳实践 ---访问日志大洞察
- vnc方式登陆服务器
- 轻松学排序算法:眼睛直观感受几种常用排序算法
- 十二个经典的大数据项目
- 为什么使用 CDN 内容分发网络?
- 大数据——大数据默认端口号列表
- Weld 1.1.5.Final,JSR-299 的框架
- JavaFX 2012:彻底开源
- 提升as3程序性能的十大要点
- 通过凸面几何学进行独立于边际的在线多类学习
- 利用行动影响的规律性和部分已知的模型进行离线强化学习
- ModelLight:基于模型的交通信号控制的元强化学习
- 浅谈Visual Source Safe项目分支
- 基于先验知识的递归卡尔曼滤波的代理人联合状态和输入估计
- 结合网络结构和非线性恢复来提高声誉评估的性能
- 最佳实践丨云开发CloudBase多环境管理实践
- TimeVAE:用于生成多变量时间序列的变异自动编码器
- 具有线性阈值激活的神经网络:结构和算法
- 内网渗透之横向移动 -- 从域外向域内进行密码喷洒攻击