g2o_solver.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Copyright (c) 2017, Saurav Agarwal
4 * All rights reserved.
5 * Modified: Steve Macenski (stevenmacenski@gmail.com)
6 *
7 *********************************************************************/
8 
9 #include "g2o_solver.hpp"
10 #include "g2o/core/block_solver.h"
11 #include "g2o/core/factory.h"
12 #include "g2o/core/robust_kernel_impl.h"
13 #include "g2o/core/optimization_algorithm_factory.h"
14 #include "g2o/core/optimization_algorithm_levenberg.h"
15 #include "g2o/types/slam2d/types_slam2d.h"
16 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
17 #include "g2o/solvers/csparse/linear_solver_csparse.h"
18 #include <karto_sdk/Karto.h>
19 #include <ros/console.h>
21 
23 
24 namespace solver_plugins
25 {
26 
27 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
28 
29 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
30 //typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
31 
32 /*****************************************************************************/
34 /*****************************************************************************/
35 {
36  // Initialize the SparseOptimizer
37  auto linearSolver = g2o::make_unique<SlamLinearSolver>();
38  linearSolver->setBlockOrdering(false);
39  auto blockSolver = g2o::make_unique<SlamBlockSolver>(
40  std::move(linearSolver));
41  optimizer_.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
42  std::move(blockSolver)));
43 
44  latestNodeID_ = 0;
45  useRobustKernel_ = true;
46 }
47 
48 /*****************************************************************************/
50 /*****************************************************************************/
51 {
52  // destroy all the singletons
53  g2o::Factory::destroy();
54  g2o::OptimizationAlgorithmFactory::destroy();
55  g2o::HyperGraphActionLibrary::destroy();
56 }
57 
58 /*****************************************************************************/
59 void G2OSolver::Clear()
60 /*****************************************************************************/
61 {
62  corrections_.clear();
63 }
64 
66 {
67  return corrections_;
68 }
69 
70 /*****************************************************************************/
72 /*****************************************************************************/
73 {
74  corrections_.clear();
75 
76  // Fix the first node in the graph to hold the map in place
77  g2o::OptimizableGraph::Vertex* first = optimizer_.vertex(0);
78 
79  if(!first)
80  {
81  ROS_ERROR("[g2o] No Node with ID 0 found!");
82  return;
83  }
84 
85  first->setFixed(true);
86 
87  // Do the graph optimization
88  const ros::Time start_time = ros::Time::now();
89  optimizer_.initializeOptimization();
90  int iter = optimizer_.optimize(500);
91  ROS_INFO("Loop Closure Solve time: %f seconds",
92  (ros::Time::now() - start_time).toSec());
93 
94  if (iter <= 0)
95  {
96  ROS_ERROR("[g2o] Optimization failed, result might be invalid!");
97  return;
98  }
99 
100  // Write the result so it can be used by the mapper
101  g2o::SparseOptimizer::VertexContainer nodes = optimizer_.activeVertices();
102  for (g2o::SparseOptimizer::VertexContainer::const_iterator n =
103  nodes.begin(); n != nodes.end(); n++)
104  {
105  double estimate[3];
106  if((*n)->getEstimateData(estimate))
107  {
108  karto::Pose2 pose(estimate[0], estimate[1], estimate[2]);
109  corrections_.push_back(std::make_pair((*n)->id(), pose));
110  }
111  else
112  {
113  ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!");
114  }
115  }
116 }
117 
118 /*****************************************************************************/
120 /*****************************************************************************/
121 {
122 
123  karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose();
124  g2o::VertexSE2* poseVertex = new g2o::VertexSE2;
125  poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(),
126  odom.GetHeading()));
127  poseVertex->setId(pVertex->GetObject()->GetUniqueId());
128  optimizer_.addVertex(poseVertex);
129  latestNodeID_ = pVertex->GetObject()->GetUniqueId();
130 
131  ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId());
132 }
133 
134 /*****************************************************************************/
136 /*****************************************************************************/
137 {
138  // Create a new edge
139  g2o::EdgeSE2* odometry = new g2o::EdgeSE2;
140 
141  // Set source and target
142  int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId();
143  int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId();
144  odometry->vertices()[0] = optimizer_.vertex(sourceID);
145  odometry->vertices()[1] = optimizer_.vertex(targetID);
146 
147  if(odometry->vertices()[0] == NULL)
148  {
149  ROS_ERROR("[g2o] Source vertex with id %d does not exist!", sourceID);
150  delete odometry;
151  return;
152  }
153 
154  if(odometry->vertices()[0] == NULL)
155  {
156  ROS_ERROR("[g2o] Target vertex with id %d does not exist!", targetID);
157  delete odometry;
158  return;
159  }
160 
161  // Set the measurement (odometry distance between vertices)
162  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
163  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
164  g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading());
165  odometry->setMeasurement(measurement);
166 
167  // Set the covariance of the measurement
168  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
169  Eigen::Matrix<double,3,3> info;
170 
171  info(0,0) = precisionMatrix(0,0);
172  info(0,1) = info(1,0) = precisionMatrix(0,1);
173  info(0,2) = info(2,0) = precisionMatrix(0,2);
174  info(1,1) = precisionMatrix(1,1);
175  info(1,2) = info(2,1) = precisionMatrix(1,2);
176  info(2,2) = precisionMatrix(2,2);
177 
178  odometry->setInformation(info);
179 
180  if(useRobustKernel_)
181  {
182  g2o::RobustKernelDCS* rk = new g2o::RobustKernelDCS;
183  odometry->setRobustKernel(rk);
184  }
185 
186  // Add the constraint to the optimizer
187  ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID);
188  optimizer_.addEdge(odometry);
189 }
190 
191 /*****************************************************************************/
192 void G2OSolver::getGraph(std::vector<Eigen::Vector2d>& nodes)
193 /*****************************************************************************/
194 {
195  double *data = new double[3];
196  for (g2o::SparseOptimizer::VertexIDMap::iterator it =
197  optimizer_.vertices().begin(); it != optimizer_.vertices().end(); ++it)
198  {
199  g2o::VertexSE2* v = dynamic_cast<g2o::VertexSE2*>(it->second);
200  if(v)
201  {
202  v->getEstimateData(data);
203  Eigen::Vector2d pose(data[0], data[1]);
204  nodes.push_back(pose);
205  }
206  }
207  delete data;
208 
209  //using namespace g2o;
210  //HyperGraph::VertexIDMap vertexMap = optimizer_.vertices();
211  //HyperGraph::EdgeSet edgeSet = optimizer_.edges();
212 
213  // double *data1 = new double[3];
214  // double *data2 = new double[3];
215  // for (SparseOptimizer::EdgeSet::iterator it = optimizer_.edges().begin();
216  // it != optimizer_.edges().end(); ++it)
217  // {
218  // EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
219  // if(e)
220  // {
221  // VertexSE2* v1 = dynamic_cast<VertexSE2*>(e->vertices()[0]);
222  // v1->getEstimateData(data1);
223  // Eigen::Vector2d poseFrom(data1[0], data1[1]);
224  // VertexSE2* v2 = dynamic_cast<VertexSE2*>(e->vertices()[1]);
225  // v2->getEstimateData(data2);
226  // Eigen::Vector2d poseTo(data2[0], data2[1]);
227  // edges.push_back(std::make_pair(poseFrom, poseTo));
228  // }
229  // }
230  // delete data1;
231  // delete data2;
232 }
233 
234 } // end namespace
karto::ScanSolver
Definition: Mapper.h:947
solver_plugins::G2OSolver::Compute
virtual void Compute()
Solve the SLAM back-end.
Definition: g2o_solver.cpp:77
solver_plugins::SlamLinearSolver
g2o::LinearSolverCholmod< SlamBlockSolver::PoseMatrixType > SlamLinearSolver
Definition: g2o_solver.cpp:35
karto::Pose2::GetY
kt_double GetY() const
Definition: Karto.h:2117
solver_plugins::G2OSolver::~G2OSolver
virtual ~G2OSolver()
Definition: g2o_solver.cpp:55
solver_plugins::G2OSolver::G2OSolver
G2OSolver()
Definition: g2o_solver.cpp:39
karto::LinkInfo::GetCovariance
const Matrix3 & GetCovariance()
Definition: Mapper.h:219
solver_plugins::G2OSolver::latestNodeID_
int latestNodeID_
Definition: g2o_solver.hpp:103
karto::Matrix3
Definition: Karto.h:2444
solver_plugins::G2OSolver::Clear
virtual void Clear()
Clear the vector of corrections.
Definition: g2o_solver.cpp:65
karto::ScanSolver::IdPoseVector
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:953
class_list_macros.h
console.h
karto::Matrix3::Inverse
Matrix3 Inverse() const
Definition: Karto.h:2545
karto::Vertex::GetObject
T * GetObject() const
Definition: Mapper.h:319
Karto.h
g2o_solver.hpp
karto::LinkInfo
Definition: Mapper.h:141
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
solver_plugins::G2OSolver::AddConstraint
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
Add an edge constraint to pose-graph.
Definition: g2o_solver.cpp:141
ROS_DEBUG
#define ROS_DEBUG(...)
solver_plugins::G2OSolver::AddNode
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
Add a node to pose-graph.
Definition: g2o_solver.cpp:125
karto::Edge::GetLabel
EdgeLabel * GetLabel()
Definition: Mapper.h:457
karto::LocalizedRangeScan::GetCorrectedPose
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5562
solver_plugins::G2OSolver::optimizer_
g2o::SparseOptimizer optimizer_
Definition: g2o_solver.hpp:101
solver_plugins::G2OSolver::corrections_
karto::ScanSolver::IdPoseVector corrections_
Definition: g2o_solver.hpp:99
karto::Edge::GetTarget
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
solver_plugins::G2OSolver::useRobustKernel_
bool useRobustKernel_
Definition: g2o_solver.hpp:105
ros::Time
karto::SensorData::GetUniqueId
kt_int32s GetUniqueId() const
Definition: Karto.h:5163
karto::Pose2::GetX
kt_double GetX() const
Definition: Karto.h:2099
ROS_ERROR
#define ROS_ERROR(...)
karto::Vertex< karto::LocalizedRangeScan >
solver_plugins::G2OSolver::GetCorrections
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Get the vector of corrections.
Definition: g2o_solver.cpp:71
karto::Pose2
Definition: Karto.h:2046
solver_plugins::G2OSolver
Wrapper for G2O to interface with Open Karto.
Definition: g2o_solver.hpp:26
process_constraints.data
def data
Definition: process_constraints.py:118
solver_plugins
Definition: ceres_solver.cpp:14
ROS_INFO
#define ROS_INFO(...)
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
karto::Edge
Definition: Mapper.h:247
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
solver_plugins::SlamBlockSolver
g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1 > > SlamBlockSolver
Definition: g2o_solver.cpp:33
ros::Time::now
static Time now()
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