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、注意行主还是列主的矩阵
其他的就和自动求导的一样