目录
ceres库官方教程：Non-linear Least Squares — Ceres Solver 可以照着这里的样例自己走一遍大概就差不多了，具体的用到时可以再针对性查看各模块。
官方教程中，On Derivatives — Ceres Solver有求导相关的详细介绍，Modeling Non-linear Least Squares — Ceres Solver 这里有各个类的详细介绍。
另外，备注几个可以参考的相关博客地址：Ceres Solver - 随笔分类(第2页) - JJ_S - 博客园
https://zhuanlan.zhihu.com/p/716857190
一、样例程序
现在我把官方教程中的样例自己写了一遍：
首先把CMakeLists.txt给出吧：
cmake_minimum_required(VERSION 2.8)
project(ceres_test)
set(CMAKE_BUILD_TYPE "Debug")
# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")
#pcl
#pcl库像下面这样配置即可，虽然main文件中会标红，但是编译都能过，不影响。
set(PCL_DIR "/home/xqy/SLAM/slambook2/3rdparty/pcl/install/share/pcl-1.13")
find_package(PCL 1.13.0 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(hello_world hello_world.cpp)
target_link_libraries(hello_world ${CERES_LIBRARIES} ${PCL_LIBRARIES})
add_executable(powell_function powell_function.cpp)
target_link_libraries(powell_function ${CERES_LIBRARIES} ${PCL_LIBRARIES})
add_executable(curve_fitting curve_fitting.cpp)
target_link_libraries(curve_fitting ${CERES_LIBRARIES} ${PCL_LIBRARIES})
add_executable(robust_curve_fitting robust_curve_fitting.cpp)
target_link_libraries(robust_curve_fitting ${CERES_LIBRARIES} ${PCL_LIBRARIES})
add_executable(bundle_adjustment bundle_adjustment.cpp)
target_link_libraries(bundle_adjustment ${CERES_LIBRARIES} ${PCL_LIBRARIES})后续代码写好后，直接建立build文件夹，然后进入build文件夹，cmake .. ，然后make，./hello_world即可运行该程序。
1、hello_world.cpp
（1）问题：求下面这个函数最小值：
关键是整个优化问题的构建过程和costfunction的构建。
注：在costfunction中，ceres应该是会对residual[0]（如果是一维残差）自动求平方；或者对多维残差residual[0]、residual[1]这样的自动求平方和。所以下面代码中残差就没平方。
（2）我的代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<Eigen/Dense>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include "ceres/ceres.h"
using namespace std;
// 代价函数的计算模型
struct square_cost {
// 残差的计算
template <typename T>
bool operator()(const T* const x, T* residual) const {
//这里面ceres会对残差自动平方，所以这里就没平方
residual[0] = T(10)-x[0];//或者T(10)写成10.0也行，是一样的，我这里这么些就是强调一下
return true;
}
};
//先写简单的最小二乘问题，比如拟合直线；
//再看看写个点云icp过程或后端BA
int main(int argc, char** argv)
{
double x0=0.5;
double x = x0;
ceres::Problem problem;
ceres::CostFunction* costfunction = new ceres::AutoDiffCostFunction<square_cost, 1, 1>(new square_cost());//点进去看需要用这个构造函数的参数，直接为空就报错了没有匹配的函数参数之类的
problem.AddResidualBlock(costfunction, nullptr, &x);
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<<summary.BriefReport()<<endl;
cout<<"result x: "<<x<<endl;
}（3）运行结果：
2、ceresCurveFitting.cpp
（1）这是一个《视觉slam十四讲》中的例子，问题如下：
（2）这里直接给出原代码，用以参考：
//
// Created by xiang on 18-11-19.
//
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST {
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
// 残差的计算
template<typename T>
bool operator()(
const T *const abc, // 模型参数，有3维
T *residual) const {
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
double abc[3] = {ae, be, ce};
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
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_NORMAL_CHOLESKY; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 输出到cout
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}（3）运行结果：
3、powell_function.cpp
（1）问题：
（2）我的代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<ceres/ceres.h>
#include<Eigen/Dense>
using namespace std;
struct F1{
template <typename T>
bool operator() (const T* const x1, const T* const x2, T* residual) const{
residual[0] = x1[0]+10.0*x2[0];//残差，ceres计算时，会自动平方
return true;
}
};
struct F2{
template <typename T>
bool operator() (const T* const x3, const T* const x4, T* residual) const{
residual[0] = sqrt(5.0)*(x3[0]-x4[0]);
return true;
}
};
struct F3{
template <typename T>
bool operator() (const T* const x2, const T* const x3, T* residual) const{
residual[0] = (x2[0]-2.0*x3[0])*(x2[0]-2.0*x3[0]);
return true;
}
};
struct F4{
template <typename T>
bool operator() (const T* const x1, const T* const x4, T* residual) const{
residual[0] = sqrt(10.0)*(x1[0]-x4[0])*(x1[0]-x4[0]);
return true;
}
};
int main(int argc, char** argv)
{
double x[4]={10.0,10.0,10.0,10.0};//初始值
ceres::Problem problem;
ceres::CostFunction* costfunc1= new ceres::AutoDiffCostFunction<F1, 1, 1, 1>(new F1());//后面三个：残差维数、参数块1维数、参数块2维数
problem.AddResidualBlock(costfunc1, nullptr, x, x+1);
ceres::CostFunction* costfunc2 = new ceres::AutoDiffCostFunction<F2, 1, 1, 1>(new F2());
problem.AddResidualBlock(costfunc2, nullptr, x+2, x+3);
ceres::CostFunction* costfunc3 = new ceres::AutoDiffCostFunction<F3, 1, 1, 1>(new F3());
problem.AddResidualBlock(costfunc3, nullptr, x+1, x+2);
ceres::CostFunction* costfunc4 = new ceres::AutoDiffCostFunction<F4, 1, 1, 1>(new F4());
problem.AddResidualBlock(costfunc4, nullptr, x, x+3);
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<<summary.BriefReport()<<endl;
cout<<"result: "<<endl;
for(auto i:x)
cout<<i<<" ";
}（3）运行结果：
4、curve_fitting.cpp
（1）问题如下：
这里数据点应该是可以直接用官方例程中的即可。
（2）代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<Eigen/Dense>
#include<ceres/ceres.h>
using namespace std;
const int kNumObservations = 67;
const double data[] = {
0.000000e+00, 1.133898e+00,
7.500000e-02, 1.334902e+00,
1.500000e-01, 1.213546e+00,
2.250000e-01, 1.252016e+00,
3.000000e-01, 1.392265e+00,
3.750000e-01, 1.314458e+00,
4.500000e-01, 1.472541e+00,
5.250000e-01, 1.536218e+00,
6.000000e-01, 1.355679e+00,
6.750000e-01, 1.463566e+00,
7.500000e-01, 1.490201e+00,
8.250000e-01, 1.658699e+00,
9.000000e-01, 1.067574e+00,
9.750000e-01, 1.464629e+00,
1.050000e+00, 1.402653e+00,
1.125000e+00, 1.713141e+00,
1.200000e+00, 1.527021e+00,
1.275000e+00, 1.702632e+00,
1.350000e+00, 1.423899e+00,
1.425000e+00, 1.543078e+00,
1.500000e+00, 1.664015e+00,
1.575000e+00, 1.732484e+00,
1.650000e+00, 1.543296e+00,
1.725000e+00, 1.959523e+00,
1.800000e+00, 1.685132e+00,
1.875000e+00, 1.951791e+00,
1.950000e+00, 2.095346e+00,
2.025000e+00, 2.361460e+00,
2.100000e+00, 2.169119e+00,
2.175000e+00, 2.061745e+00,
2.250000e+00, 2.178641e+00,
2.325000e+00, 2.104346e+00,
2.400000e+00, 2.584470e+00,
2.475000e+00, 1.914158e+00,
2.550000e+00, 2.368375e+00,
2.625000e+00, 2.686125e+00,
2.700000e+00, 2.712395e+00,
2.775000e+00, 2.499511e+00,
2.850000e+00, 2.558897e+00,
2.925000e+00, 2.309154e+00,
3.000000e+00, 2.869503e+00,
3.075000e+00, 3.116645e+00,
3.150000e+00, 3.094907e+00,
3.225000e+00, 2.471759e+00,
3.300000e+00, 3.017131e+00,
3.375000e+00, 3.232381e+00,
3.450000e+00, 2.944596e+00,
3.525000e+00, 3.385343e+00,
3.600000e+00, 3.199826e+00,
3.675000e+00, 3.423039e+00,
3.750000e+00, 3.621552e+00,
3.825000e+00, 3.559255e+00,
3.900000e+00, 3.530713e+00,
3.975000e+00, 3.561766e+00,
4.050000e+00, 3.544574e+00,
4.125000e+00, 3.867945e+00,
4.200000e+00, 4.049776e+00,
4.275000e+00, 3.885601e+00,
4.350000e+00, 4.110505e+00,
4.425000e+00, 4.345320e+00,
4.500000e+00, 4.161241e+00,
4.575000e+00, 4.363407e+00,
4.650000e+00, 4.161576e+00,
4.725000e+00, 4.619728e+00,
4.800000e+00, 4.737410e+00,
4.875000e+00, 4.727863e+00,
4.950000e+00, 4.669206e+00,
};
struct curve_cost{
curve_cost(double x, double y):_x(x), _y(y){}
template <typename T>
bool operator() (const T* const para, T* residual) const {
residual[0] = exp(para[0]*_x + para[1]) - _y;
return true;
}
double _x, _y;
};
int main(int argc, char** argv)
{
double para[2]={1.0, 1.0};//m,c
ceres::Problem problem;
for(int i=0; i<kNumObservations; i++)
{
ceres::CostFunction* costfunction = new ceres::AutoDiffCostFunction<curve_cost, 1, 2>(new curve_cost(data[i*2], data[i*2+1]));
problem.AddResidualBlock(costfunction, nullptr, para);
}
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<<summary.BriefReport()<<endl;
cout<<"result: m: "<<para[0]<<" c: "<<para[1]<<endl;
}（3）运行结果：
5、robust_curve_fitting.cpp
（1）问题：与上面4、的区别就是数据点中加了两个明显偏离曲线的外点，然后在添加残差块的时候可以使用核函数lossfunction，使得残差大的时候权重降低来处理。
这里把官方教程的问题说明搬运过来：
（2）我的代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<Eigen/Dense>
#include<ceres/ceres.h>
using namespace std;
const int kNumObservations = 67;
const double data[] = {
0.000000e+00, 1.133898e+00,
7.500000e-02, 1.334902e+00,
1.500000e-01, 1.213546e+00,
2.250000e-01, 1.252016e+00,
3.000000e-01, 1.392265e+00,
3.750000e-01, 1.314458e+00,
4.500000e-01, 1.472541e+00,
5.250000e-01, 1.536218e+00,
6.000000e-01, 1.355679e+00,
6.750000e-01, 1.463566e+00,
7.500000e-01, 1.490201e+00,
8.250000e-01, 1.658699e+00,
9.000000e-01, 1.067574e+00,
9.750000e-01, 1.464629e+00,
1.050000e+00, 1.402653e+00,
1.125000e+00, 1.713141e+00,
1.200000e+00, 1.527021e+00,
1.275000e+00, 1.702632e+00,
1.350000e+00, 1.423899e+00,
1.425000e+00, 5.543078e+00, // Outlier point
1.500000e+00, 5.664015e+00, // Outlier point
1.575000e+00, 1.732484e+00,
1.650000e+00, 1.543296e+00,
1.725000e+00, 1.959523e+00,
1.800000e+00, 1.685132e+00,
1.875000e+00, 1.951791e+00,
1.950000e+00, 2.095346e+00,
2.025000e+00, 2.361460e+00,
2.100000e+00, 2.169119e+00,
2.175000e+00, 2.061745e+00,
2.250000e+00, 2.178641e+00,
2.325000e+00, 2.104346e+00,
2.400000e+00, 2.584470e+00,
2.475000e+00, 1.914158e+00,
2.550000e+00, 2.368375e+00,
2.625000e+00, 2.686125e+00,
2.700000e+00, 2.712395e+00,
2.775000e+00, 2.499511e+00,
2.850000e+00, 2.558897e+00,
2.925000e+00, 2.309154e+00,
3.000000e+00, 2.869503e+00,
3.075000e+00, 3.116645e+00,
3.150000e+00, 3.094907e+00,
3.225000e+00, 2.471759e+00,
3.300000e+00, 3.017131e+00,
3.375000e+00, 3.232381e+00,
3.450000e+00, 2.944596e+00,
3.525000e+00, 3.385343e+00,
3.600000e+00, 3.199826e+00,
3.675000e+00, 3.423039e+00,
3.750000e+00, 3.621552e+00,
3.825000e+00, 3.559255e+00,
3.900000e+00, 3.530713e+00,
3.975000e+00, 3.561766e+00,
4.050000e+00, 3.544574e+00,
4.125000e+00, 3.867945e+00,
4.200000e+00, 4.049776e+00,
4.275000e+00, 3.885601e+00,
4.350000e+00, 4.110505e+00,
4.425000e+00, 4.345320e+00,
4.500000e+00, 4.161241e+00,
4.575000e+00, 4.363407e+00,
4.650000e+00, 4.161576e+00,
4.725000e+00, 4.619728e+00,
4.800000e+00, 4.737410e+00,
4.875000e+00, 4.727863e+00,
4.950000e+00, 4.669206e+00
};
struct curve_cost{
curve_cost(double x, double y):_x(x), _y(y){}
template <typename T>
bool operator() (const T* const para, T* residual) const {
residual[0] = exp(para[0]*_x + para[1]) - _y;
return true;
}
double _x, _y;
};
int main(int argc, char** argv)
{
double para[2]={1.0, 1.0};//m,c
ceres::Problem problem;
for(int i=0; i<kNumObservations; i++)
{
ceres::CostFunction* costfunction = new ceres::AutoDiffCostFunction<curve_cost, 1, 2>(new curve_cost(data[i*2], data[i*2+1]));
ceres::LossFunction* lossfunction = new ceres::CauchyLoss(0.5);
problem.AddResidualBlock(costfunction, lossfunction, para);//用lossfunction来另残差高的权重低，减小外点的影响
}
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<<summary.BriefReport()<<endl;
cout<<"result: m: "<<para[0]<<" c: "<<para[1]<<endl;
//注：真实值：m:0.3 c:0.1 数据中有两个外点
//不加lossfunction的话，输出结果：m: 0.254001 c: 0.291741
//加lossfunction，输出结果:m: 0.287534 c: 0.151526
}（3）运行结果：
6、bundle_adjustment.cpp
（1）问题分析：这个是要使用Bundle Adjustment in the Large的数据，来做bundle adjustment，我这里使用了Bundle Adjustment in the Large: Dubrovnik Dataset (washington.edu)中的序列problem-16-22106-pre.txt.bz2 来实验。
具体而言，就是给出了相机数，3d点（路标）数，相机观测3d点从而产生的观测2d点数（图像坐标系中）。然后给出了对应的观测到的2d点坐标、每个相机索引对应的参数（外参（旋转（角轴/旋转向量表示）、平移）、焦距、径向畸变系数）。然后要通过构建重投影误差，来一起优化所有相机的上述参数和所有3d点(路标)坐标。
这里对于使用相机参数（外参（旋转（角轴/旋转向量表示）、平移）、焦距、径向畸变系数）将一个3d点投影到图像坐标系，并去畸变的过程在数据集网站上给出了：
大概就是：世界坐标系下3d点 经过外参变为 相机坐标系下3d点。然后除以-z变到归一化坐标系下。再然后算径向距离dis，从而去畸变（或者说畸变）；最后经过焦距，转到图像坐标系下得到对应2d点的x、y坐标。
然后跟对应的观测值相减，即可构建重投影误差，这里在ceres中只需要给出residual[0]（对应x）、residual[1]（对应y），后续ceres应该是会自动计算平方和算残差大小。
下面给出该数据集中的数据组织格式，参考网址：BAL(Bundle Adjustment in the Large) - C洼，如下：
<slambook2>/ch9/problem-16-22106-pre.txt
==================
=={1/4}相机数(位姿数) 路标个数 观测方程个数
16 22106 83718
=={2/4}观测方程。记录第i个相机观测第j个路标所看到的像素坐标（投影）
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
7 0 -5.991800e+02 4.079300e+02
12 0 -7.204300e+02 3.143400e+02
13 0 -1.151300e+02 5.548999e+01
0 1 3.838800e+02 -1.529999e+01
1 1 5.597500e+02 -1.061500e+02
10 1 3.531899e+02 1.649500e+02
0 2 5.915500e+02 1.364400e+02
1 2 8.638600e+02 -2.346997e+01
2 2 4.947200e+02 1.125200e+02
6 2 4.087800e+02 2.846700e+02
7 2 4.246100e+02 3.101700e+02
9 2 2.848900e+02 1.928900e+02
10 2 5.826200e+02 3.637200e+02
12 2 4.940601e+02 2.939500e+02
13 2 7.968300e+02 -7.853003e+01
15 2 7.798900e+02 4.082500e+02
......
14 22103 1.808000e+02 -5.225200e+02
15 22103 -2.593700e+02 4.393600e+02
14 22104 8.235999e+01 -5.621600e+02
15 22104 -3.864900e+02 3.803600e+02
14 22105 2.212600e+02 -4.689200e+02
15 22105 -1.963200e+02 5.126800e+02
=={3/4}相机参数。一个相机9参数：旋转向量(3)、平移(3)、焦距(1)、畸变(2)。（144行）
[相机#0的9参数]
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
[相机#1的9参数]
1.5049725341485708e-02
-1.8504564785154357e-01
-2.9278402790141456e-01
-1.0590476152349551e+00
-3.6017862414345798e-02
-1.5720340175803784e+00
1.4321374541298685e+03
-7.3171919892612292e-08
3.1759419019880947e-14
...
=={4/4}路标在空间坐标系的三维坐标
[路标#0的坐标]
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01
[路标#2的坐标]
6.4168905904672933e+00
3.8897031177598462e-01
-2.3586282709150449e+01
...
[路标#22105的坐标]
-3.7719680164031812e+00
1.1022501903417394e+01
-4.7349911639646528e+01（2）然后给出我的代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<fstream>
#include<ceres/ceres.h>
#include<ceres/rotation.h>
using namespace std;
struct camera_observe{
camera_observe(){}
camera_observe(int idpoint, double x, double y):idpoint(idpoint), _x(x), _y(y){
}
int idpoint;
double _x;
double _y;
};
class camera_para_observe{
public:
camera_para_observe(){}
vector<camera_observe> observe;
double r[3];
double t[3];
double f;
double dis_k[2];
};
struct camara_reprojection_cost{
camara_reprojection_cost(double x, double y):_x_oberserve(x), _y_oberserve(y){}
//做重投影误差
//problem.AddResidualBlock(costfunction, nullptr, &r, &t, &f, &k, &p_3d)
template <typename T>
bool operator() (const T* const r, const T* const t, const T* const f, const T* const k_dis, const T* const point3d, T* residual) const{
//3d点投影到图像坐标系，并去畸变
//world系->camera系
T point3d_c[3];
ceres::AngleAxisRotatePoint(r, point3d, point3d_c);
point3d_c[0] += t[0];
point3d_c[1] += t[1];
point3d_c[2] += t[2];
//camera系->归一化坐标系（单纯除以z即可）
T point2d_projection[2];
point2d_projection[0] = -point3d_c[0]/point3d_c[2];
point2d_projection[1] = -point3d_c[1]/point3d_c[2];
T rdis = point2d_projection[0]*point2d_projection[0] + point2d_projection[1]*point2d_projection[1];//径向距离的平方
//去畸变
point2d_projection[0] = point2d_projection[0] * (T(1.0)+k_dis[0]*rdis + k_dis[1]*rdis*rdis);
point2d_projection[1] = point2d_projection[1] * (T(1.0)+k_dis[0]*rdis + k_dis[1]*rdis*rdis);
//归一化坐标系(去畸变后)->图像坐标系
point2d_projection[0] = point2d_projection[0]*f[0];
point2d_projection[1] = point2d_projection[1]*f[0];
//计算残差
residual[0] = point2d_projection[0] - T(_x_oberserve);
residual[1] = point2d_projection[1] - T(_y_oberserve);
//cout<<"residual: "<< residual[0]<<" 1"<< residual[1]<<" "<<residual[0]*residual[0]+residual[1]*residual[1]<<endl;
return true;
}
//观测作为参数
double _x_oberserve, _y_oberserve;
};
int main()
{
//读入txt文件，得到3d点、相机、观测的信息
string path("../data");
ifstream file;
file.open(path);
int cameraNum, point3dNum, oberserveNum;
file>>cameraNum>>point3dNum>>oberserveNum;
//其实两种存储方式：一种直接存，另一种就是创建个相机类，存储相机的参数和它的观测。这里弄个类吧
vector<camera_para_observe> cam_data(cameraNum);
double point3d[point3dNum][3];//注：这里不用vector<vector<double>>的原因是下面AddResidualBlock中
//直接用这个会报错：显示参数块是这个类型std::vector<double, std::allocator<double>>
//而参数块一般直接用double就行。
//read observation
for(int i=0; i<oberserveNum; i++)
{
int camera_id, pointid;
double x, y;
file>>camera_id>>pointid>>x>>y;
camera_observe obcur(pointid, x, y);
cam_data[camera_id].observe.push_back(obcur);
//cout<<"idpoint: "<<cam_data[camera_id].observe.back().idpoint<<endl;
}
//read camera parameter
for(int i=0; i<cameraNum; i++)
{
file>>cam_data[i].r[0]>>cam_data[i].r[1]>>cam_data[i].r[2];
file>>cam_data[i].t[0]>>cam_data[i].t[1]>>cam_data[i].t[2];
file>>cam_data[i].f;
file>>cam_data[i].dis_k[0]>>cam_data[i].dis_k[1];
}
//read point3d
for(int i=0; i<point3dNum; i++)
{
file>>point3d[i][0]>>point3d[i][1]>>point3d[i][2];
//cout << "Reading 3D point: " << point3d[i][0] << " " << point3d[i][1] << " " << point3d[i][2] << endl;
}
cout<<"try read data: "<<point3d[0][0]<<endl;
cout<<"finish read data!"<<endl;
/*
for(int i=0; i<cameraNum; i++)
{
cout<<"i obersevationnum: "<<cam_data[i].observe.size()<<endl;
}
*/
//构建优化问题
ceres::Problem problem;
//观测数
for(int i=0; i<cameraNum; i++)
{
for(int j=0; j<cam_data[i].observe.size(); j++)
{
double xcur_observe = cam_data[i].observe[j]._x;
double ycur_observe = cam_data[i].observe[j]._y;
//residual维数、r、t、f、k、point3d各自维数
ceres::CostFunction* costfunction =
new ceres::AutoDiffCostFunction<camara_reprojection_cost, 2, 3, 3, 1, 2, 3>(new camara_reprojection_cost(xcur_observe, ycur_observe));
//ceres::LossFunction* lossfunction = new ceres::HuberLoss(1.0);//这里如果使用huberloss，优化时cost值会更系小，但后续无法收敛，不用反而很快就收敛了，虽然cost大一点。
//旋转、平移、焦距f、畸变k1,k2、当前3d点
//problem.AddResidualBlock(costfunction, nullptr, &r, &t, &f, &k, &p_3d)
problem.AddResidualBlock(costfunction, nullptr, cam_data[i].r, cam_data[i].t,
&cam_data[i].f, cam_data[i].dis_k, point3d[cam_data[i].observe[j].idpoint]);
}
}
ceres::Solver::Options options;
//这里若使用DENSE_QR就一直没结果，换成DENSE_SCHUR就3s左右就开始有了
options.linear_solver_type = ceres::DENSE_SCHUR;//后续可以改这里看效果，以及上面添加lossfunction再对比看看
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//cout<<summary.BriefReport()<<endl;
cout<<summary.FullReport()<<endl;
}（3）运行结果：
二、ICP
这部分使用斯坦福的bunny数据集，地址为：
选择0度和90度视角下的兔子点云，进行icp配准。
0度：
90度：
1、point-to-point ICP
使用点到点的残差来做，这里结果会跟初值设置有关，代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<Eigen/Dense>
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<ceres/ceres.h>
#include<ceres/rotation.h>
//之后删
//#include<pcl-1.10/pcl/io/pcd_io.h>
//#include<pcl-1.10/pcl/point_types.h>
//#include<pcl-1.10/pcl/point_cloud.h>
using namespace std;
struct p2p_cost{
p2p_cost(double* point_sour, double* point_tar)
{
_point_sour[0] = point_sour[0];
_point_sour[1] = point_sour[1];
_point_sour[2] = point_sour[2];
_point_tar[0] = point_tar[0];
_point_tar[1] = point_tar[1];
_point_tar[2] = point_tar[2];
/*
_point_sour = Eigen::Map<const Eigen::Vector3d>(point_sour);
_point_tar = Eigen::Map<const Eigen::Vector3d>(point_tar);
*/
}
template <typename T>
bool operator() (const T* const r, const T* const t, T* residual) const{
T point_tar_map[3];
T point_target[3];
for(int i=0; i<3; i++)
point_target[i] = T(_point_tar[i]);
ceres::AngleAxisRotatePoint(r, point_target, point_tar_map);
/*
Eigen::Matrix<T, 3, 1> point_tar_cast = _point_tar.cast<T>();
cout<<point_tar_cast.data()<<endl;
ceres::AngleAxisRotatePoint(r, point_tar_cast.data(), point_tar_map);
*/
//cout<<point_tar_map[0]<<' '<<point_tar_map[1]<<' '<<point_tar_map[2]<<endl;
point_tar_map[0] += t[0];
point_tar_map[1] += t[1];
point_tar_map[2] += t[2];
residual[0] = point_tar_map[0] - T(_point_sour[0]);
residual[1] = point_tar_map[1] - T(_point_sour[1]);
residual[2] = point_tar_map[2] - T(_point_sour[2]);
return true;
}
double _point_sour[3];
double _point_tar[3];
//Eigen::Vector3d _point_sour;
//Eigen::Vector3d _point_tar;
};
//点到点的icp在这里对相差90度的两个bunny,target变到source坐标系下，最后结果会跟初值相关，但不管初值设在哪(就算在
//真值r=(0,1.57,0)处)，都无法到最优结果，应该是因为这两个bunny的部分不完全一致？
int main() {
//源点云读入
string source_path = "../bunny/bun000.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr source( new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile<pcl::PointXYZ>(source_path, *source);
cout << "source point num: " << source->points.size()<<endl;
cout<<source->points[0].x<<" "<<source->points[0].y<<" "<<source->points[0].z<<endl;
pcl::visualization::CloudViewer viewer_source("bunny_origin");
viewer_source.showCloud(source);
while (!viewer_source.wasStopped())
{
}
//目标点云读入
string target_path = "../bunny/bun090.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile<pcl::PointXYZ>(target_path, *target);
cout << "target point num: " << target->size()<<endl;
pcl::visualization::CloudViewer viewer_target("bunny_target");
viewer_target.showCloud(target);
while (!viewer_target.wasStopped())
{
}
//icp过程点云配准，知道source跟target之间的位姿变换
//法一：ceres实现优化过程 法二：调用pcl的icp模块实现 法三：手写高斯-牛顿优化
//这里用法一来实现
//定义优化的位姿参数 rp+t将target中的点变换到source坐标系中，即r和t是target在source下的位姿。
double r[3] = {0,1.1,0};//旋转的角轴
double t[3] = {0,0,0};//平移
//构建source的kd tree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(source);
int k_search = 1;
int max_iteration = 40;
for(int j=0; j<max_iteration; j++)
{
ceres::Problem problem;
//对target每个点
for(int i=0; i<target->points.size(); i++)
{
double target_origin[3] = {target->points[i].x, target->points[i].y, target->points[i].z};
double target_final[3];
ceres::AngleAxisRotatePoint(r, target_origin, target_final);//用角轴来旋转，相当于R*p
target_final[0] += t[0];
target_final[1] += t[1];
target_final[2] += t[2];
pcl::PointXYZ target_map(target_final[0], target_final[1], target_final[2]);
//cout<<target->points[i].x<<" "<<target->points[i].y<<" "<<target->points[i].z;
//cout<<" end: "<<target_map.x<<" "<<target_map.y<<" "<<target_map.z<<endl;
//k search
vector<int> pointIdx_nearest(k_search);
vector<float> pointSquareDistance(k_search);
if(kdtree.nearestKSearch(target_map, k_search, pointIdx_nearest, pointSquareDistance))
{
if(pointSquareDistance[k_search-1]>1.0)//距离太远就不可靠
continue;
int id = pointIdx_nearest[0];
//cout<<"id: "<<id<<" ";
double point_sour[3] = {source->points[id].x, source->points[id].y, source->points[id].z};
//add residual block
ceres::CostFunction* costfunction
= new ceres::AutoDiffCostFunction<p2p_cost, 3, 3, 3>(new p2p_cost(point_sour, target_origin));
problem.AddResidualBlock(costfunction, nullptr, r, t);
//cout<<i<<"distance: "<< pointSquareDistance[k_search-1]<<endl;
}
}
cout<<"finish add residual block!"<<endl;
cout<<"r: "<<r[0]<<" "<<r[1]<<" "<<r[2]<<endl;
cout<<"t: "<<t[0]<<" "<<t[1]<<" "<<t[2]<<endl;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
//options.function_tolerance = 1e-15;
//options.gradient_tolerance = 1e-6;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout<<summary.BriefReport()<<endl;
cout<<"r: "<<r[0]<<" "<<r[1]<<" "<<r[2]<<endl;
cout<<"t: "<<t[0]<<" "<<t[1]<<" "<<t[2]<<endl;
}
//得到将target变换到source系下的点云结果
pcl::PointCloud<pcl::PointXYZ>::Ptr target_so(new pcl::PointCloud<pcl::PointXYZ>());
target_so->points.resize(target->points.size());
cout<<target_so->points.size()<<endl;
for(int i=0; i<target->points.size(); i++)
{
double target_origin[3] = {target->points[i].x, target->points[i].y, target->points[i].z};
double target_final[3];
ceres::AngleAxisRotatePoint(r, target_origin, target_final);
target_final[0] += t[0];
target_final[1] += t[1];
target_final[2] += t[2];
pcl::PointXYZ pi(target_final[0], target_final[1], target_final[2]);
target_so->points[i] = pi;
}
pcl::visualization::CloudViewer viewer_final("target_in_source");
viewer_final.showCloud(target_so);
while (!viewer_final.wasStopped())
{
}
}结果：
将target下的点云变换到source下，这里设置迭代40次，结果为：
可见结果不够准确。
2、point-to-plane ICP
使用点到面的残差来做，这里结果会跟初值设置有关，这里初值设置为跟1、point-to-point ICP中一样，迭代10次，看最后结果如何。代码如下：
#include<iostream>
#include<algorithm>
#include<vector>
#include<Eigen/Dense>
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<ceres/ceres.h>
#include<ceres/rotation.h>
//之后删
//#include<pcl-1.10/pcl/io/pcd_io.h>
//#include<pcl-1.10/pcl/point_types.h>
//#include<pcl-1.10/pcl/point_cloud.h>
using namespace std;
struct p2plane_cost{
p2plane_cost(double* plane_equa, double* point_tar)
{
_plane_equa[0] = plane_equa[0];
_plane_equa[1] = plane_equa[1];
_plane_equa[2] = plane_equa[2];
_point_tar[0] = point_tar[0];
_point_tar[1] = point_tar[1];
_point_tar[2] = point_tar[2];
/*
_point_sour = Eigen::Map<const Eigen::Vector3d>(point_sour);
_point_tar = Eigen::Map<const Eigen::Vector3d>(point_tar);
*/
}
template <typename T>
bool operator() (const T* const r, const T* const t, T* residual) const{
T point_tar_map[3];
T point_target[3];
for(int i=0; i<3; i++)
point_target[i] = T(_point_tar[i]);
ceres::AngleAxisRotatePoint(r, point_target, point_tar_map);
point_tar_map[0] += t[0];
point_tar_map[1] += t[1];
point_tar_map[2] += t[2];
residual[0] = (T(_plane_equa[0])*point_tar_map[0] + T(_plane_equa[1])*point_tar_map[1]+ T(_plane_equa[2])*point_tar_map[2] + T(1.0))
/ sqrt(T(_plane_equa[0])*T(_plane_equa[0]) + T(_plane_equa[1])*T(_plane_equa[1]) + T(_plane_equa[2])*T(_plane_equa[2]));
return true;
}
double _plane_equa[3];
double _point_tar[3];
};
int main() {
//源点云读入
string source_path = "../bunny/bun000.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr source( new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile<pcl::PointXYZ>(source_path, *source);
cout << "source point num: " << source->points.size()<<endl;
cout<<source->points[0].x<<" "<<source->points[0].y<<" "<<source->points[0].z<<endl;
pcl::visualization::CloudViewer viewer_source("bunny_origin");
viewer_source.showCloud(source);
while (!viewer_source.wasStopped())
{
}
//目标点云读入
string target_path = "../bunny/bun090.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile<pcl::PointXYZ>(target_path, *target);
cout << "target point num: " << target->size()<<endl;
pcl::visualization::CloudViewer viewer_target("bunny_target");
viewer_target.showCloud(target);
while (!viewer_target.wasStopped())
{
}
//icp过程点云配准，知道source跟target之间的位姿变换
//法一：ceres实现优化过程 法二：调用pcl的icp模块实现 法三：手写高斯-牛顿优化
//这里用法一来实现
//定义优化的位姿参数 rp+t将target中的点变换到source坐标系中，即r和t是target在source下的位姿。
double r[3] = {0,1.1,0};//旋转的角轴
double t[3] = {0,0,0};//平移
//构建source的kd tree
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(source);
int k_search = 5;
int max_iteration = 10;
for(int j=0; j<max_iteration; j++)
{
ceres::Problem problem;
//对target每个点
for(int i=0; i<target->points.size(); i++)
{
double target_origin[3] = {target->points[i].x, target->points[i].y, target->points[i].z};
double target_final[3];
ceres::AngleAxisRotatePoint(r, target_origin, target_final);//用角轴来旋转，相当于R*p
target_final[0] += t[0];
target_final[1] += t[1];
target_final[2] += t[2];
pcl::PointXYZ target_map(target_final[0], target_final[1], target_final[2]);
//cout<<target->points[i].x<<" "<<target->points[i].y<<" "<<target->points[i].z;
//cout<<" end: "<<target_map.x<<" "<<target_map.y<<" "<<target_map.z<<endl;
//k search
vector<int> pointIdx_nearest(k_search);
vector<float> pointSquareDistance(k_search);
if(kdtree.nearestKSearch(target_map, k_search, pointIdx_nearest, pointSquareDistance))
{
if(pointSquareDistance[k_search-1]>1.0)//距离太远就不可靠
continue;
//计算平面方程 Ax+By+Cz+1=0
Eigen::Matrix<double, 5, 3> matrixA;
for(int k=0; k<5; k++)
{
int id = pointIdx_nearest[k];
matrixA(k,0) = source->points[id].x;
matrixA(k,1) = source->points[id].y;
matrixA(k,2) = source->points[id].z;
}
Eigen::Matrix<double, 5, 1> matrixb;
matrixb.fill(-1);
matrixb<<-1,-1,-1,-1,-1;
Eigen::Vector3d matrixX = matrixA.colPivHouseholderQr().solve(matrixb);
//验证是否构成平面：计算每个平面点到平面的距离，如果过大，认为不靠谱
bool planeValid = true;
for(int k=0; k<5; k++)
{
double dis = (matrixA(k,0)*matrixX(0) + matrixA(k,1)*matrixX(1) + matrixA(k,2)*matrixX(2) + 1.0) /
sqrt(matrixX(0)*matrixX(0) + matrixX(1)*matrixX(1) + matrixX(2)*matrixX(2));
if(dis>0.2)
{
planeValid = false;
continue;
}
}
if(planeValid)
{
double plane_equa[3] = {matrixX(0), matrixX(1), matrixX(2)};
//cout<<matrixX(0)<<' '<<plane_equa[0]<<endl;
//add residual block
ceres::CostFunction* costfunction
= new ceres::AutoDiffCostFunction<p2plane_cost, 1, 3, 3>(new p2plane_cost(plane_equa, target_origin));
problem.AddResidualBlock(costfunction, nullptr, r, t);
//cout<<i<<"distance: "<< pointSquareDistance[k_search-1]<<endl;
}
}
}
cout<<"finish add residual block!"<<endl;
cout<<"r: "<<r[0]<<" "<<r[1]<<" "<<r[2]<<endl;
cout<<"t: "<<t[0]<<" "<<t[1]<<" "<<t[2]<<endl;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
//options.function_tolerance = 1e-15;
//options.gradient_tolerance = 1e-6;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout<<summary.BriefReport()<<endl;
cout<<"r: "<<r[0]<<" "<<r[1]<<" "<<r[2]<<endl;
cout<<"t: "<<t[0]<<" "<<t[1]<<" "<<t[2]<<endl;
}
//得到将target变换到source系下的点云结果
pcl::PointCloud<pcl::PointXYZ>::Ptr target_so(new pcl::PointCloud<pcl::PointXYZ>());
target_so->points.resize(target->points.size());
cout<<target_so->points.size()<<endl;
for(int i=0; i<target->points.size(); i++)
{
double target_origin[3] = {target->points[i].x, target->points[i].y, target->points[i].z};
double target_final[3];
ceres::AngleAxisRotatePoint(r, target_origin, target_final);
target_final[0] += t[0];
target_final[1] += t[1];
target_final[2] += t[2];
pcl::PointXYZ pi(target_final[0], target_final[1], target_final[2]);
target_so->points[i] = pi;
}
pcl::visualization::CloudViewer viewer_final("target_in_source");
viewer_final.showCloud(target_so);
while (!viewer_final.wasStopped())
{
}
}结果如下：
结果还是不够准确，之后有空再看看怎么改进或哪有问题吧。
扫一扫
197
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
