ceres关于图优化问题
Posted Libra
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了ceres关于图优化问题相关的知识,希望对你有一定的参考价值。
ceres关于图优化问题
首先是图的节点,一般为位姿;再者,边代表节点与节点之间的相对变换(旋转和平移),一般是真实测量的数据,如里程计、激光雷达数据、imu数据等。如下图,三角形代表位姿、边代表测量数据;虚线代表回环检测的约束边。
#include <fstream> #include <iostream> #include <map> #include <string> #include <vector> #include "angle_local_parameterization.h" #include "ceres/ceres.h" #include "common/read_g2o.h" #include "gflags/gflags.h" #include "glog/logging.h" #include "pose_graph_2d_error_term.h" #include "types.h" DEFINE_string(input, "", "The pose graph definition filename in g2o format."); namespace ceres { namespace examples { namespace { // Constructs the nonlinear least squares optimization problem from the pose // graph constraints. void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints, std::map<int, Pose2d>* poses, ceres::Problem* problem) { CHECK(poses != NULL); CHECK(problem != NULL); if (constraints.empty()) { LOG(INFO) << "No constraints, no problem to optimize."; return; } ceres::LossFunction* loss_function = NULL; ceres::LocalParameterization* angle_local_parameterization = AngleLocalParameterization::Create(); //创建参数项 for (std::vector<Constraint2d>::const_iterator constraints_iter = constraints.begin(); constraints_iter != constraints.end(); ++constraints_iter) { const Constraint2d& constraint = *constraints_iter; std::map<int, Pose2d>::iterator pose_begin_iter = poses->find(constraint.id_begin); CHECK(pose_begin_iter != poses->end()) << "Pose with ID: " << constraint.id_begin << " not found."; std::map<int, Pose2d>::iterator pose_end_iter = poses->find(constraint.id_end); CHECK(pose_end_iter != poses->end()) << "Pose with ID: " << constraint.id_end << " not found."; const Eigen::Matrix3d sqrt_information = constraint.information.llt().matrixL(); // Ceres will take ownership of the pointer. ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create( constraint.x, constraint.y, constraint.yaw_radians, sqrt_information); //传参进去 problem->AddResidualBlock(cost_function, loss_function, &pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians, &pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians); //添加残差项 传入优化变量 problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); problem->SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization); } // The pose graph optimization problem has three DOFs that are not fully // constrained. This is typically referred to as gauge freedom. You can apply // a rigid body transformation to all the nodes and the optimization problem // will still have the exact same cost. The Levenberg-Marquardt algorithm has // internal damping which mitigate this issue, but it is better to properly // constrain the gauge freedom. This can be done by setting one of the poses // as constant so the optimizer cannot change it. std::map<int, Pose2d>::iterator pose_start_iter = poses->begin(); CHECK(pose_start_iter != poses->end()) << "There are no poses."; problem->SetParameterBlockConstant(&pose_start_iter->second.x); problem->SetParameterBlockConstant(&pose_start_iter->second.y); problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians); } // Returns true if the solve was successful. bool SolveOptimizationProblem(ceres::Problem* problem) { CHECK(problem != NULL); ceres::Solver::Options options; options.max_num_iterations = 100; options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; ceres::Solver::Summary summary; ceres::Solve(options, problem, &summary); std::cout << summary.FullReport() << \'\\n\'; return summary.IsSolutionUsable(); } // Output the poses to the file with format: ID x y yaw_radians. bool OutputPoses(const std::string& filename, const std::map<int, Pose2d>& poses) { std::fstream outfile; outfile.open(filename.c_str(), std::istream::out); if (!outfile) { std::cerr << "Error opening the file: " << filename << \'\\n\'; return false; } for (std::map<int, Pose2d>::const_iterator poses_iter = poses.begin(); poses_iter != poses.end(); ++poses_iter) { const std::map<int, Pose2d>::value_type& pair = *poses_iter; outfile << pair.first << " " << pair.second.x << " " << pair.second.y << \' \' << pair.second.yaw_radians << \'\\n\'; } return true; } } // namespace } // namespace examples } // namespace ceres int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); CHECK(FLAGS_input != "") << "Need to specify the filename to read."; std::map<int, ceres::examples::Pose2d> poses; std::vector<ceres::examples::Constraint2d> constraints; CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints)) << "Error reading the file: " << FLAGS_input; std::cout << "Number of poses: " << poses.size() << \'\\n\'; std::cout << "Number of constraints: " << constraints.size() << \'\\n\'; CHECK(ceres::examples::OutputPoses("poses_original.txt", poses)) << "Error outputting to poses_original.txt"; ceres::Problem problem; ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem); CHECK(ceres::examples::SolveOptimizationProblem(&problem)) << "The solve was not successful, exiting."; CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses)) << "Error outputting to poses_original.txt"; return 0; }
以上是关于ceres关于图优化问题的主要内容,如果未能解决你的问题,请参考以下文章