Wrapper for G2O to interface with Open Karto. More...
#include <GTSAM_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... | |
virtual const karto::ScanSolver::IdPoseVector & | GetCorrections () const |
Get the vector of corrections. More... | |
virtual void | getGraph (std::vector< Eigen::Vector2d > &nodes) |
Get the pose-graph. More... | |
GTSAMSolver () | |
virtual void | ModifyNode (const int &unique_id, const Eigen::Vector3d &pose) |
virtual | ~GTSAMSolver () |
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_ |
gtsam::NonlinearFactorGraph | graph_ |
std::vector< Eigen::Vector2d > | graphNodes_ |
gtsam::Values | initialGuess_ |
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 32 of file GTSAM_solver.hpp.
solver_plugins::GTSAMSolver::GTSAMSolver | ( | ) |
Definition at line 28 of file GTSAM_solver.cpp.
|
virtual |
Definition at line 42 of file GTSAM_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 120 of file GTSAM_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 103 of file GTSAM_solver.cpp.
|
virtual |
Clear the vector of corrections.
Empty out previously computed corrections
Reimplemented from karto::ScanSolver.
Definition at line 49 of file GTSAM_solver.cpp.
|
virtual |
Solve the SLAM back-end.
Calls G2O to solve the SLAM back-end
Implements karto::ScanSolver.
Definition at line 63 of file GTSAM_solver.cpp.
|
virtual |
Get the vector of corrections.
Get the vector of corrections
Implements karto::ScanSolver.
Definition at line 56 of file GTSAM_solver.cpp.
|
virtual |
Get the pose-graph.
Get the underlying graph from g2o, return the graph of constraints
g | the graph |
Definition at line 165 of file GTSAM_solver.cpp.
|
virtual |
|
private |
Definition at line 94 of file GTSAM_solver.hpp.
|
private |
Definition at line 95 of file GTSAM_solver.hpp.
|
private |
Definition at line 97 of file GTSAM_solver.hpp.
|
private |
Definition at line 96 of file GTSAM_solver.hpp.