Graph-based global alignement using g2o (generalized graph optimizaiton) More...
#include <keyframe_graph_solver_g2o.h>
Public Member Functions | |
KeyframeGraphSolverG2O (const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) | |
Constructor from ROS noehandles. | |
void | solve (KeyframeVector &keyframes, KeyframeAssociationVector &associations) |
Main method to call to perform graph solving using g2o. | |
~KeyframeGraphSolverG2O () | |
Default destructor. | |
Private Member Functions | |
void | addEdge (int from_idx, int to_idx, const Eigen::Matrix4f &relative_pose, const Eigen::Matrix< double, 6, 6 > &information_matrix) |
Adds an edge to the g2o structure. | |
void | addVertex (const Eigen::Matrix4f &vertex_pose, int vertex_idx) |
Adds a vertex to the g2o structure. | |
void | optimizeGraph () |
runs the optimization | |
void | updatePoses (KeyframeVector &keyframes) |
copies the (optimized) poses from the g2o structure into the keyframe vector | |
Private Attributes | |
g2o::BlockSolverX::LinearSolverType * | linearSolver |
g2o::SparseOptimizer | optimizer |
g2o::BlockSolverX * | solver_ptr |
int | vertexIdx |
Graph-based global alignement using g2o (generalized graph optimizaiton)
Definition at line 42 of file keyframe_graph_solver_g2o.h.
ccny_rgbd::KeyframeGraphSolverG2O::KeyframeGraphSolverG2O | ( | const ros::NodeHandle & | nh, |
const ros::NodeHandle & | nh_private | ||
) |
Constructor from ROS noehandles.
nh | the public nodehandle |
nh_private | the private notehandle |
Definition at line 28 of file keyframe_graph_solver_g2o.cpp.
Default destructor.
Definition at line 43 of file keyframe_graph_solver_g2o.cpp.
void ccny_rgbd::KeyframeGraphSolverG2O::addEdge | ( | int | from_idx, |
int | to_idx, | ||
const Eigen::Matrix4f & | relative_pose, | ||
const Eigen::Matrix< double, 6, 6 > & | information_matrix | ||
) | [private] |
Adds an edge to the g2o structure.
Definition at line 130 of file keyframe_graph_solver_g2o.cpp.
void ccny_rgbd::KeyframeGraphSolverG2O::addVertex | ( | const Eigen::Matrix4f & | vertex_pose, |
int | vertex_idx | ||
) | [private] |
Adds a vertex to the g2o structure.
Definition at line 95 of file keyframe_graph_solver_g2o.cpp.
void ccny_rgbd::KeyframeGraphSolverG2O::optimizeGraph | ( | ) | [private] |
runs the optimization
Definition at line 166 of file keyframe_graph_solver_g2o.cpp.
void ccny_rgbd::KeyframeGraphSolverG2O::solve | ( | KeyframeVector & | keyframes, |
KeyframeAssociationVector & | associations | ||
) | [virtual] |
Main method to call to perform graph solving using g2o.
keyframes | vector of keyframes |
associations | vector of input keyframe associations |
Implements ccny_rgbd::KeyframeGraphSolver.
Definition at line 48 of file keyframe_graph_solver_g2o.cpp.
void ccny_rgbd::KeyframeGraphSolverG2O::updatePoses | ( | KeyframeVector & | keyframes | ) | [private] |
copies the (optimized) poses from the g2o structure into the keyframe vector
keyframes | the vector of keyframes to modify the poses |
Definition at line 178 of file keyframe_graph_solver_g2o.cpp.
g2o::BlockSolverX::LinearSolverType* ccny_rgbd::KeyframeGraphSolverG2O::linearSolver [private] |
Definition at line 69 of file keyframe_graph_solver_g2o.h.
g2o::SparseOptimizer ccny_rgbd::KeyframeGraphSolverG2O::optimizer [private] |
Definition at line 68 of file keyframe_graph_solver_g2o.h.
g2o::BlockSolverX* ccny_rgbd::KeyframeGraphSolverG2O::solver_ptr [private] |
Definition at line 70 of file keyframe_graph_solver_g2o.h.
int ccny_rgbd::KeyframeGraphSolverG2O::vertexIdx [private] |
Definition at line 67 of file keyframe_graph_solver_g2o.h.