ceres是用来求解优化问题的工具库。
使用时至少需要构建目标函数，优化变量。
如果自动求导困难，则需要给出雅克比矩阵。（一般是采用了第三方的库，比如eigen和sophus的一些运算）
如果优化变量不对加法封闭，则需要给出优化变量的更新方法。
==============================================================================
对于简单优化问题总体流程：（可以采用自动求导）
1、构建代价函数模型，描述如何计算残差
2、构建问题（要给一个初值）
3、配置求解器
4、结果输出
以两张图片的pnp优化为例：
//ceres pnp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <chrono>
cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K );
void find_feature_matches (
const cv::Mat& img_1, const cv::Mat& img_2,
std::vector<cv::KeyPoint>& keypoints_1,
std::vector<cv::KeyPoint>& keypoints_2,
std::vector<cv::DMatch>& matches );
struct cost_function
{
cost_function(cv::Point3f p1,cv::Point2f p2):pworld(p1),pimg(p2) {}
template <typename T>
bool operator()(const T* const ceres_r, const T* const ceres_t, T* residual) const
{
T p_world[3];
T p_cam[3];
p_world[0] = T(pworld.x);
p_world[1] = T(pworld.y);
p_world[2] = T(pworld.z);
ceres::AngleAxisRotatePoint(ceres_r,p_world,p_cam);
p_cam[0] = p_cam[0] + ceres_t[0];
p_cam[1] = p_cam[1] + ceres_t[1];
p_cam[2] = p_cam[2] + ceres_t[2];
// std::cout<< p_cam[0] << p_cam[1] << p_cam[2] <<std::endl;
const T x = p_cam[0] / p_cam[2];
const T y = p_cam[1] / p_cam[2];
const T u = x * T(K.at<double>(0, 0)) + T(K.at<double>(0, 2));
const T v = y * T(K.at<double>(1, 1)) + T(K.at<double>(1, 2));
T u1 = T(pimg.x);
T v1 = T(pimg.y);
residual[0] = u - u1;
residual[1] = v - v1;
return true;
}
private:
cv::Point3f pworld;
cv::Point2f pimg;
};
int main(int argc, char **argv) {
//-- 读取图像
cv::Mat img_1 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/1.png", CV_LOAD_IMAGE_COLOR);
cv::Mat img_2 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/2.png", CV_LOAD_IMAGE_COLOR);
assert(img_1.data && img_2.data && "Can not load images!");
std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
std::vector<cv::DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
std::cout << "一共找到了" << matches.size() << "组匹配点" << std::endl;
// 建立3D点
cv::Mat d1 = cv::imread("/home/smz/learning_ref/slambook2-master/ch7/1_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度图为16位无符号数，单通道图像
std::vector<cv::Point3f> pts_3d;
std::vector<cv::Point2f> pts_2d;
for (cv::DMatch m:matches) {
ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if (d == 0) // bad depth
continue;
float dd = d / 5000.0;
cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(cv::Point3f(p1.x * dd, p1.y * dd, dd));
pts_2d.push_back(keypoints_2[m.trainIdx].pt);
}
std::cout << "3d-2d pairs: " << pts_3d.size() << std::endl;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
cv::Mat r, t;
solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
cv::Mat R;
cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
std::cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << std::endl;
std::cout << "R=" << std::endl << R << std::endl;
std::cout << "t=" << std::endl << t << std::endl;
double ceres_rot[3],ceres_tran[3],ceres_rho[6];
ceres_rot[0] = 0;
ceres_rot[1] = 1;
ceres_rot[2] = 2;
ceres_tran[0] = t.at<double>(0,0)+0.1;
ceres_tran[1] = t.at<double>(1,0)+0.1;
ceres_tran[2] = t.at<double>(2,0)+0.1;
ceres::Problem problem;
for(int i = 0; i < pts_3d.size();i++)
{
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<cost_function,2,3,3>(new cost_function(pts_3d[i],pts_2d[i])),nullptr,ceres_rot,ceres_tran);
}
ceres::Solver::Options option;
option.linear_solver_type = ceres::DENSE_SCHUR;
option.minimizer_progress_to_stdout=true;
ceres::Solver::Summary summary;
t1 = std::chrono::steady_clock::now();
ceres::Solve(option,&problem,&summary);
t2 = std::chrono::steady_clock::now();
time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
std::cout << summary.BriefReport() << std::endl;
std::cout << "solve pnp by ceres cost time: " << time_used.count() << " seconds." << std::endl;
cv::Mat cam_3d = ( cv::Mat_<double> ( 3,1 )<<ceres_rot[0],ceres_rot[1],ceres_rot[2]);
cv::Mat cam_9d;
cv::Rodrigues (cam_3d, cam_9d);
std::cout << "cam_9d:" << std::endl << cam_9d << std::endl;
std::cout << "cam_t:" << ceres_tran[0] << " " << ceres_tran[1] << " " << ceres_tran[2] << std::endl;
return 0;
}
void find_feature_matches(const cv::Mat &img_1, const cv::Mat &img_2,
std::vector<cv::KeyPoint> &keypoints_1,
std::vector<cv::KeyPoint> &keypoints_2,
std::vector<cv::DMatch> &matches) {
//-- 初始化
cv::Mat descriptors_1, descriptors_2;
// used in OpenCV3
cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
std::vector<cv::DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= cv::max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K) {
return cv::Point2d
(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
基本上可以简述为：
struct cost_function //构建代价函数
{
cost_function(cv::Point3f p1,cv::Point2f p2):pworld(p1),pimg(p2) {}//将非优化变量输入
template <typename T> //定义模板函数
bool operator()(const T* const ceres_r, const T* const ceres_t, T* residual) const
{
//给出非优化变量和优化变量应该如何计算得到最后的结果（残差），其中优化变量应该是以数组形式输入，残差也需要表示为数组格式
T p_world[3];
T p_cam[3];
p_world[0] = T(pworld.x);
p_world[1] = T(pworld.y);
p_world[2] = T(pworld.z);
ceres::AngleAxisRotatePoint(ceres_r,p_world,p_cam);
p_cam[0] = p_cam[0] + ceres_t[0];
p_cam[1] = p_cam[1] + ceres_t[1];
p_cam[2] = p_cam[2] + ceres_t[2];
// std::cout<< p_cam[0] << p_cam[1] << p_cam[2] <<std::endl;
const T x = p_cam[0] / p_cam[2];
const T y = p_cam[1] / p_cam[2];
const T u = x * T(K.at<double>(0, 0)) + T(K.at<double>(0, 2));
const T v = y * T(K.at<double>(1, 1)) + T(K.at<double>(1, 2));
T u1 = T(pimg.x);
T v1 = T(pimg.y);
residual[0] = u - u1;
residual[1] = v - v1;
return true;
}
private:
cv::Point3f pworld;
cv::Point2f pimg;
}; ceres::Problem problem; //构建优化问题
for(int i = 0; i < pts_3d.size();i++)
{
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<cost_function,2,3,3>(new cost_function(pts_3d[i],pts_2d[i])),nullptr,ceres_rot,ceres_tran);//给出每个问题的误差
}==============================================================================
复杂一点的需要给参数更新方式和雅克比矩阵
// 利用ceres实现位姿图优化，位姿图的输入是一个球，数据在文件sphere.g2o中 数据结构如下
/* sphere.g2o 结构
数据：
VERTEX_SE3:QUAT 2499 1.0318 85.323 -76.0615 0.779315 0.0718127 -0.620437 -0.0506864
EDGE_SE3:QUAT 0 1 -0.0187953 0.0328449 -0.125146 0.0634648 -0.000250128 0.00237634 0.997981 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 40000 0 0 40000 0 40000
顶点：
数据中以VERTEX_SE3:QUAT开头的为顶点，结构为VERTEX_SE3:QUAT 节点id 平移 四元数
边：
以EDGE_SE3:QUAT 开头的为边，结构为 EDGE_SE3:QUAT 节点id1 节点id2 平移 四元数 信息矩阵上三角（6*6的信息矩阵，上三角为6*6/2+6/2=21）
*/
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
#include <ceres/ceres.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
typedef Eigen::Matrix<double,6,6> Matrix6d;
typedef Eigen::Matrix<double,6,1> Vector6d;
class mypoint
{
public:
mypoint(int _id1, double x,double y,double z ,double q0 ,double q1 ,double q2 ,double q3){
id1 = _id1;
param[0] = x;
param[1] = y;
param[2] = z;
param[3] = q0;
param[4] = q1;
param[5] = q2;
param[6] = q3;
con_param2se3();
}
int id1;
double se3[6];
private:
void con_param2se3();
double param[7];
};
void mypoint::con_param2se3()
{
Eigen::Quaterniond q(param[3], param[4], param[5], param[6]);
q.normalize();
Eigen::Matrix<double,6,1> se = Sophus::SE3d( q, Eigen::Vector3d(param[0], param[1], param[2]) ).log();
for(int i = 0; i < 6; i++)
{
se3[i] = se(i,0);
}
}
class SE3Parameterization : public ceres::LocalParameterization
{
public:
SE3Parameterization() {}
virtual ~SE3Parameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const
{
Eigen::Map<const Eigen::Matrix<double, 6, 1>> varse(x);
Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_varse(delta);
Sophus::SE3d T = Sophus::SE3d::exp(varse);
Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_varse);
Vector6d x_plus_delta_varse = (delta_T * T).log();
for(int i = 0; i < 6; ++i)
x_plus_delta[i] = x_plus_delta_varse(i, 0);
return true;
}
virtual bool ComputeJacobian(const double* x,
double* jacobian) const
{
ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
return true;
}
virtual int GlobalSize() const { return 6; }
virtual int LocalSize() const { return 6; }
};
class PoseGraphCostFunction : public ceres::SizedCostFunction<6,6,6>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
~PoseGraphCostFunction(){}
PoseGraphCostFunction(Sophus::SE3d _se3, Matrix6d _covariance): edge_se3(_se3), convariance(_covariance){}
virtual bool Evaluate(double const* const* parameters,
double *residuals,
double **jacobians) const{
Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
Sophus::SE3d pose_j = Sophus::SE3d::exp(Vector6d(parameters[1]));
Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
// Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
Eigen::Map<Vector6d> residual(residuals);
// residual = (edge_se3.inverse() * estimate).log();
residual = (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
if(jacobians){
Matrix6d Jr;
Jr.block(0,0,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).so3().log());
Jr.block(0,3,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).translation());
Jr.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
Jr.block(3,3,3,3) = Jr.block(0,0,3,3);
Jr = Matrix6d::Identity() + 0.5 * Jr ;
Matrix6d jacA = - Jr * pose_j.inverse().Adj();
Matrix6d jacB = - jacA;
if(jacobians[0])
{
Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);
jacobian_i = sqrt_info * jacA;
}
if(jacobians[1])
{
Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);
jacobian_j = sqrt_info * jacB;
}
}
residual = sqrt_info * (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
return true;
}
private:
const Sophus::SE3d edge_se3;
const Eigen::Matrix<double,6,6> convariance;
};
void drawpclpoint(std::vector<mypoint> points)
{
pcl::PointCloud<pcl::PointXYZRGB> ::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );
for(int i = 0; i < points.size(); i++ )
{
pcl::PointXYZRGB pointpcl;
Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d( points[i].se3 );
Sophus::SE3d poseSE3 = Sophus::SE3d::exp(poseAVec6d);
Eigen::Matrix<double,3,1> point_t = poseSE3.translation().transpose();
pointpcl.x = point_t(0,0);
pointpcl.y = point_t(1,0);
pointpcl.z = point_t(2,0);
pointpcl.b = 0;
pointpcl.g = 0;
pointpcl.r = 255;
pointCloud->points.push_back( pointpcl );
}
// pointCloud->is_dense = true;
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(pointCloud);
while (!viewer.wasStopped())
{
}
}
int main(int argc, char** argv)
{
const std::string file_path = "/home/smz/learning_ref/slambook2-master/ch10/sphere.g2o";
std::ifstream g2oFile(file_path);
if(!g2oFile)
{
std::cout << "file " << file_path << "doesnot exist" << std::endl;
return -1;
}
ceres::Problem problem;
ceres::LocalParameterization *local_param = new SE3Parameterization();
int pose_num = 0;
int edge_num = 0;
std::vector<mypoint> points;
while(!g2oFile.eof())
{
std::string flag;
g2oFile >> flag;
if(flag[0] == 'V')
{
pose_num++;
int id;
double x,y,z,q0,q1,q2,q3;
g2oFile >> id >> x >> y >> z >> q1 >> q2 >> q3 >> q0;
mypoint p(id,x,y,z,q0,q1,q2,q3);
points.push_back(p);
}
if(flag[0] == 'E')
{
edge_num++;
int id1,id2;
double x,y,z,q0,q1,q2,q3;
g2oFile >> id1 >> id2 >> x >> y >> z >> q1 >> q2 >> q3 >> q0;
Sophus::SE3d edge_m(Eigen::Quaterniond(q0, q1, q2, q3).normalized(),Eigen::Vector3d(x, y, z));
Eigen::Matrix<double,6,6> information;
for(int i = 0;i < 6; i++)
{
for(int j = i;j < 6; j++)
{
g2oFile >> information(i,j);
if(j != i)
information(j, i) = information(i, j);
}
}
ceres::LossFunction *loss = new ceres::HuberLoss(1.0);
ceres::CostFunction *costfunc = new PoseGraphCostFunction(edge_m, information);
problem.AddResidualBlock(costfunc, loss, points[id1].se3, points[id2].se3);
problem.SetParameterization(points[id1].se3, local_param);
problem.SetParameterization(points[id2].se3, local_param);
}
}
g2oFile.close();
drawpclpoint(points);
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_linear_solver_iterations = 50;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
std::ofstream txt("./result.g2o");
for( int i=0; i < points.size(); i++ )
{
Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d( points[i].se3 );
Sophus::SE3d poseSE3 = Sophus::SE3d::exp(poseAVec6d);
Eigen::Quaterniond q = poseSE3.so3().unit_quaternion();
txt << "VERTEX_SE3:QUAT" << ' ';
txt << i << ' ';
txt << poseSE3.translation().transpose() << ' ';
txt << q.x() <<' '<< q.y()<< ' ' << q.z() <<' '<< q.w()<<' ' << std::endl;
}
g2oFile.open(file_path);
while(!g2oFile.eof()){
std::string s;
std::getline(g2oFile, s);
if(s[0] != 'E') continue;
else txt << s << std::endl;
}
g2oFile.close();
txt.close();
drawpclpoint(points);
return 0;
}
其中class SE3Parameterization : public ceres::LocalParameterization 构建了优化变量
class SE3Parameterization : public ceres::LocalParameterization
{
public:
SE3Parameterization() {}
virtual ~SE3Parameterization() {}
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const
{
Eigen::Map<const Eigen::Matrix<double, 6, 1>> varse(x);
Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_varse(delta);
Sophus::SE3d T = Sophus::SE3d::exp(varse);
Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_varse);
Vector6d x_plus_delta_varse = (delta_T * T).log();
for(int i = 0; i < 6; ++i)
x_plus_delta[i] = x_plus_delta_varse(i, 0);
return true;
}
virtual bool ComputeJacobian(const double* x,
double* jacobian) const
{
ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
return true;
}
virtual int GlobalSize() const { return 6; }
virtual int LocalSize() const { return 6; }
};
输入是变量，增量，变量+增量，用来构建更新方式，同时给出雅克比矩阵，最后给出变量维度和正切空间维度
class PoseGraphCostFunction : public ceres::SizedCostFunction<6,6,6>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
~PoseGraphCostFunction(){}
PoseGraphCostFunction(Sophus::SE3d _se3, Matrix6d _covariance): edge_se3(_se3), convariance(_covariance){}
virtual bool Evaluate(double const* const* parameters,
double *residuals,
double **jacobians) const{
Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
Sophus::SE3d pose_j = Sophus::SE3d::exp(Vector6d(parameters[1]));
Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
// Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
Eigen::Map<Vector6d> residual(residuals);
// residual = (edge_se3.inverse() * estimate).log();
residual = (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
if(jacobians){
Matrix6d Jr;
Jr.block(0,0,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).so3().log());
Jr.block(0,3,3,3) = Sophus::SO3d::hat(Sophus::SE3d::exp(residual).translation());
Jr.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
Jr.block(3,3,3,3) = Jr.block(0,0,3,3);
Jr = Matrix6d::Identity() + 0.5 * Jr ;
Matrix6d jacA = - Jr * pose_j.inverse().Adj();
Matrix6d jacB = - jacA;
if(jacobians[0])
{
Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);
jacobian_i = sqrt_info * jacA;
}
if(jacobians[1])
{
Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);
jacobian_j = sqrt_info * jacB;
}
}
residual = sqrt_info * (edge_se3.inverse() * pose_i.inverse() * pose_j).log();
return true;
}
private:
const Sophus::SE3d edge_se3;
const Eigen::Matrix<double,6,6> convariance;
};
注意：1、所有变量都是在parameters中，输入的时候需要将其分解分别保存
2、注意行主还是列主的矩阵
其他的就和自动求导的一样