Wrapper for G2O to interface with Open Karto. More...
#include <g2o_solver.hpp>
Public Member Functions | |
virtual void | AddConstraint (karto::Edge< karto::LocalizedRangeScan > *pEdge) |
Add an edge constraint to pose-graph. More... | |
virtual void | AddNode (karto::Vertex< karto::LocalizedRangeScan > *pVertex) |
Add a node to pose-graph. More... | |
virtual void | Clear () |
Clear the vector of corrections. More... | |
virtual void | Compute () |
Solve the SLAM back-end. More... | |
G2OSolver () | |
virtual const karto::ScanSolver::IdPoseVector & | GetCorrections () const |
Get the vector of corrections. More... | |
void | getGraph (std::vector< Eigen::Vector2d > &nodes) |
Get the pose-graph. More... | |
virtual void | ModifyNode (const int &unique_id, const Eigen::Vector3d &pose) |
void | useRobustKernel (bool flag) |
Use robust kernel in back-end. More... | |
virtual | ~G2OSolver () |
Public Member Functions inherited from karto::ScanSolver | |
virtual std::unordered_map< int, Eigen::Vector3d > * | getGraph () |
virtual void | GetNodeOrientation (const int &unique_id, double &pose) |
virtual void | ModifyNode (const int &unique_id, Eigen::Vector3d pose) |
virtual void | RemoveConstraint (kt_int32s, kt_int32s) |
virtual void | RemoveNode (kt_int32s) |
virtual void | Reset () |
ScanSolver () | |
template<class Archive > | |
void | serialize (Archive &ar, const unsigned int version) |
virtual | ~ScanSolver () |
Private Attributes | |
karto::ScanSolver::IdPoseVector | corrections_ |
int | latestNodeID_ |
g2o::SparseOptimizer | optimizer_ |
bool | useRobustKernel_ |
Additional Inherited Members | |
Public Types inherited from karto::ScanSolver | |
typedef std::vector< std::pair< kt_int32s, Pose2 > > | IdPoseVector |
Wrapper for G2O to interface with Open Karto.
Definition at line 26 of file g2o_solver.hpp.
solver_plugins::G2OSolver::G2OSolver | ( | ) |
Definition at line 39 of file g2o_solver.cpp.
|
virtual |
Definition at line 55 of file g2o_solver.cpp.
|
virtual |
Add an edge constraint to pose-graph.
Adds a relative pose measurement constraint between two poses in the graph
pEdge | [description] |
Reimplemented from karto::ScanSolver.
Definition at line 141 of file g2o_solver.cpp.
|
virtual |
Add a node to pose-graph.
Add a node which is a robot pose to the pose-graph
pVertex | the node to be added in |
Reimplemented from karto::ScanSolver.
Definition at line 125 of file g2o_solver.cpp.
|
virtual |
Clear the vector of corrections.
Empty out previously computed corrections
Reimplemented from karto::ScanSolver.
Definition at line 65 of file g2o_solver.cpp.
|
virtual |
Solve the SLAM back-end.
Calls G2O to solve the SLAM back-end
Implements karto::ScanSolver.
Definition at line 77 of file g2o_solver.cpp.
|
virtual |
Get the vector of corrections.
Get the vector of corrections
Implements karto::ScanSolver.
Definition at line 71 of file g2o_solver.cpp.
void solver_plugins::G2OSolver::getGraph | ( | std::vector< Eigen::Vector2d > & | nodes | ) |
Get the pose-graph.
Get the underlying graph from g2o, return the graph of constraints
g | the graph |
Definition at line 198 of file g2o_solver.cpp.
|
virtual |
|
inline |
Use robust kernel in back-end.
Uses Dynamic Covariance scaling kernel in back-end
flag | variable, if true robust kernel will be used |
Definition at line 90 of file g2o_solver.hpp.
|
private |
Definition at line 99 of file g2o_solver.hpp.
|
private |
Definition at line 103 of file g2o_solver.hpp.
|
private |
Definition at line 101 of file g2o_solver.hpp.
|
private |
Definition at line 105 of file g2o_solver.hpp.