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"
27 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> >
SlamBlockSolver;
29 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType>
SlamLinearSolver;
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)));
53 g2o::Factory::destroy();
54 g2o::OptimizationAlgorithmFactory::destroy();
55 g2o::HyperGraphActionLibrary::destroy();
77 g2o::OptimizableGraph::Vertex* first =
optimizer_.vertex(0);
81 ROS_ERROR(
"[g2o] No Node with ID 0 found!");
85 first->setFixed(
true);
91 ROS_INFO(
"Loop Closure Solve time: %f seconds",
96 ROS_ERROR(
"[g2o] Optimization failed, result might be invalid!");
101 g2o::SparseOptimizer::VertexContainer nodes =
optimizer_.activeVertices();
102 for (g2o::SparseOptimizer::VertexContainer::const_iterator n =
103 nodes.begin(); n != nodes.end(); n++)
106 if((*n)->getEstimateData(estimate))
108 karto::Pose2 pose(estimate[0], estimate[1], estimate[2]);
109 corrections_.push_back(std::make_pair((*n)->id(), pose));
113 ROS_ERROR(
"[g2o] Could not get estimated pose from Optimizer!");
124 g2o::VertexSE2* poseVertex =
new g2o::VertexSE2;
125 poseVertex->setEstimate(g2o::SE2(odom.
GetX(), odom.
GetY(),
139 g2o::EdgeSE2* odometry =
new g2o::EdgeSE2;
144 odometry->vertices()[0] =
optimizer_.vertex(sourceID);
145 odometry->vertices()[1] =
optimizer_.vertex(targetID);
147 if(odometry->vertices()[0] == NULL)
149 ROS_ERROR(
"[g2o] Source vertex with id %d does not exist!", sourceID);
154 if(odometry->vertices()[0] == NULL)
156 ROS_ERROR(
"[g2o] Target vertex with id %d does not exist!", targetID);
165 odometry->setMeasurement(measurement);
169 Eigen::Matrix<double,3,3> info;
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);
178 odometry->setInformation(info);
182 g2o::RobustKernelDCS* rk =
new g2o::RobustKernelDCS;
183 odometry->setRobustKernel(rk);
187 ROS_DEBUG(
"[g2o] Adding Edge from node %d to node %d.", sourceID, targetID);
195 double *
data =
new double[3];
196 for (g2o::SparseOptimizer::VertexIDMap::iterator it =
199 g2o::VertexSE2* v =
dynamic_cast<g2o::VertexSE2*
>(it->second);
202 v->getEstimateData(
data);
203 Eigen::Vector2d pose(
data[0],
data[1]);
204 nodes.push_back(pose);