GTSAM_solver.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Copyright (c) 2017, Saurav Agarwal
4 * All rights reserved.
5 *
6 *********************************************************************/
7 
8 /* Authors: Saurav Agarwal */
9 /* Modified: Steve Macenski */
10 
11 #include <limits>
12 #include <karto_sdk/Karto.h>
13 #include <ros/console.h>
14 #include "GTSAM_solver.hpp"
16 
18 
19 namespace solver_plugins
20 {
21 
22 /*****************************************************************************/
24 /*****************************************************************************/
25 {
26  using namespace gtsam;
27 
28  // add the prior on the first node which is known
29  noiseModel::Diagonal::shared_ptr priorNoise =
30  noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
31 
32  graph_.emplace_shared<PriorFactor<Pose2> >(0, Pose2(0, 0, 0), priorNoise);
33 
34 }
35 
36 /*****************************************************************************/
38 /*****************************************************************************/
39 {
40 
41 }
42 
43 /*****************************************************************************/
44 void GTSAMSolver::Clear()
45 /*****************************************************************************/
46 {
47  corrections_.clear();
48 }
49 
50 /*****************************************************************************/
52 /*****************************************************************************/
53 {
54  return corrections_;
55 }
56 
57 /*****************************************************************************/
59 /*****************************************************************************/
60 {
61  using namespace gtsam;
62 
63  corrections_.clear();
64 
65  graphNodes_.clear();
66 
67  LevenbergMarquardtParams parameters;
68 
69  // Stop iterating once the change in error between steps is less than this value
70  parameters.relativeErrorTol = 1e-5;
71 
72  // Do not perform more than N iteration steps
73  parameters.maxIterations = 500;
74 
75  // Create the optimizer ...
76  LevenbergMarquardtOptimizer optimizer(graph_, initialGuess_, parameters);
77 
78  // ... and optimize
79  Values result = optimizer.optimize();
80 
81  Values::ConstFiltered<Pose2> viewPose2 = result.filter<Pose2>();
82 
83  // put values into corrections container
84  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2)
85  {
86 
87  karto::Pose2 pose(key_value.value.x(), key_value.value.y(),
88  key_value.value.theta());
89 
90  corrections_.push_back(std::make_pair(key_value.key, pose));
91 
92  graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(),
93  key_value.value.y()));
94  }
95 }
96 
97 /*****************************************************************************/
99 /*****************************************************************************/
100 {
101  using namespace gtsam;
102 
104 
105  initialGuess_.insert(pVertex->GetObject()->GetUniqueId(),
106  Pose2( odom.GetX(), odom.GetY(), odom.GetHeading() ));
107 
108  graphNodes_.push_back(Eigen::Vector2d(odom.GetX(), odom.GetY()));
109 
110  ROS_DEBUG("[gtsam] Adding node %d.", pVertex->GetObject()->GetUniqueId());
111 
112 }
113 
114 /*****************************************************************************/
116 /*****************************************************************************/
117 {
118  using namespace gtsam;
119 
120  // Set source and target
121  int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
122 
123  int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
124 
125  // Set the measurement (poseGraphEdge distance between vertices)
126  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
127 
128  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
129 
130  // Set the covariance of the measurement
131  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance();
132 
133  Eigen::Matrix<double,3,3> cov;
134 
135  cov(0,0) = precisionMatrix(0,0);
136 
137  cov(0,1) = cov(1,0) = precisionMatrix(0,1);
138 
139  cov(0,2) = cov(2,0) = precisionMatrix(0,2);
140 
141  cov(1,1) = precisionMatrix(1,1);
142 
143  cov(1,2) = cov(2,1) = precisionMatrix(1,2);
144 
145  cov(2,2) = precisionMatrix(2,2);
146 
147  noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov);
148 
149  // Add odometry factors
150  // Create odometry (Between) factors between consecutive poses
151  graph_.emplace_shared<BetweenFactor<Pose2> >(sourceID, targetID,
152  Pose2(diff.GetX(), diff.GetY(), diff.GetHeading()), model);
153 
154  // Add the constraint to the optimizer
155  ROS_DEBUG("[gtsam] Adding Edge from node %d to node %d.", sourceID, targetID);
156 
157 }
158 
159 /*****************************************************************************/
160 void GTSAMSolver::getGraph(std::vector<Eigen::Vector2d>& nodes)
161 /*****************************************************************************/
162 {
163  nodes = graphNodes_;
164  // using namespace gtsam;
165  // double *data1 = new double[3];
166  // double *data2 = new double[3];
167  // for (SparseOptimizer::EdgeSet::iterator it =
168  // optimizer_.edges().begin(); it != optimizer_.edges().end(); ++it)
169  // {
170  // EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
171  // if(e)
172  // {
173  // VertexSE2* v1 = dynamic_cast<VertexSE2*>(e->vertices()[0]);
174  // v1->getEstimateData(data1);
175  // Eigen::Vector2d poseFrom(data1[0], data1[1]);
176  // VertexSE2* v2 = dynamic_cast<VertexSE2*>(e->vertices()[1]);
177  // v2->getEstimateData(data2);
178  // Eigen::Vector2d poseTo(data2[0], data2[1]);
179  // edges.push_back(std::make_pair(poseFrom, poseTo));
180  // }
181  // }
182  // delete data1;
183  // delete data2;
184 }
185 
186 } // end namespace
solver_plugins::GTSAMSolver::GetCorrections
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Get the vector of corrections.
Definition: GTSAM_solver.cpp:56
karto::ScanSolver
Definition: Mapper.h:947
solver_plugins::GTSAMSolver::AddConstraint
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
Add an edge constraint to pose-graph.
Definition: GTSAM_solver.cpp:120
solver_plugins::GTSAMSolver::GTSAMSolver
GTSAMSolver()
Definition: GTSAM_solver.cpp:28
karto::Pose2::GetY
kt_double GetY() const
Definition: Karto.h:2117
solver_plugins::GTSAMSolver::graph_
gtsam::NonlinearFactorGraph graph_
Definition: GTSAM_solver.hpp:95
GTSAM_solver.hpp
karto::LinkInfo::GetCovariance
const Matrix3 & GetCovariance()
Definition: Mapper.h:219
karto::Matrix3
Definition: Karto.h:2444
karto::ScanSolver::IdPoseVector
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:953
class_list_macros.h
console.h
karto::Vertex::GetObject
T * GetObject() const
Definition: Mapper.h:319
Karto.h
karto::LinkInfo
Definition: Mapper.h:141
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ROS_DEBUG
#define ROS_DEBUG(...)
solver_plugins::GTSAMSolver::graphNodes_
std::vector< Eigen::Vector2d > graphNodes_
Definition: GTSAM_solver.hpp:97
Vector3
karto::Edge::GetLabel
EdgeLabel * GetLabel()
Definition: Mapper.h:457
karto::LocalizedRangeScan::GetCorrectedPose
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5562
solver_plugins::GTSAMSolver::AddNode
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
Add a node to pose-graph.
Definition: GTSAM_solver.cpp:103
solver_plugins::GTSAMSolver
Wrapper for G2O to interface with Open Karto.
Definition: GTSAM_solver.hpp:32
karto::Edge::GetTarget
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
karto::SensorData::GetUniqueId
kt_int32s GetUniqueId() const
Definition: Karto.h:5163
solver_plugins::GTSAMSolver::~GTSAMSolver
virtual ~GTSAMSolver()
Definition: GTSAM_solver.cpp:42
karto::Pose2::GetX
kt_double GetX() const
Definition: Karto.h:2099
karto::Vertex< karto::LocalizedRangeScan >
karto::Pose2
Definition: Karto.h:2046
solver_plugins::GTSAMSolver::corrections_
karto::ScanSolver::IdPoseVector corrections_
Definition: GTSAM_solver.hpp:94
solver_plugins
Definition: ceres_solver.cpp:14
karto::ScanSolver::getGraph
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Definition: Mapper.h:1026
karto::Edge::GetSource
Vertex< T > * GetSource() const
Definition: Mapper.h:439
solver_plugins::GTSAMSolver::Compute
virtual void Compute()
Solve the SLAM back-end.
Definition: GTSAM_solver.cpp:63
karto::Edge
Definition: Mapper.h:247
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
solver_plugins::GTSAMSolver::initialGuess_
gtsam::Values initialGuess_
Definition: GTSAM_solver.hpp:96
solver_plugins::GTSAMSolver::Clear
virtual void Clear()
Clear the vector of corrections.
Definition: GTSAM_solver.cpp:49
karto::LinkInfo::GetPoseDifference
const Pose2 & GetPoseDifference()
Definition: Mapper.h:210


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55