spa_solver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include "spa_solver.hpp"
19 #include <karto_sdk/Karto.h>
20 
21 #include "ros/console.h"
23 
25 
26 namespace solver_plugins
27 {
28 typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
29 
30 /*****************************************************************************/
32 /*****************************************************************************/
33 {
34 
35 }
36 
37 /*****************************************************************************/
39 /*****************************************************************************/
40 {
41 
42 }
43 
44 /*****************************************************************************/
46 /*****************************************************************************/
47 {
48  corrections.clear();
49 }
50 
52 {
53  return corrections;
54 }
55 
56 /*****************************************************************************/
58 /*****************************************************************************/
59 {
60  corrections.clear();
61 
62  const ros::Time start_time = ros::Time::now();
63  m_Spa.doSPA(40, 1.0e-4, 1);
64  ROS_INFO("Loop Closure Solve time: %f seconds",
65  (ros::Time::now() - start_time).toSec());
66 
67  NodeVector nodes = m_Spa.getNodes();
68  forEach(NodeVector, &nodes)
69  {
70  karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
71  corrections.push_back(std::make_pair(iter->nodeId, pose));
72  }
73 }
74 
75 /*****************************************************************************/
77 /*****************************************************************************/
78 {
79  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
80  Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
81  m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
82 }
83 
84 /*****************************************************************************/
86 /*****************************************************************************/
87 {
88  karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
89  karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
90  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
91 
92  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
93  Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
94 
95  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
96  Eigen::Matrix<double,3,3> m;
97  m(0,0) = precisionMatrix(0,0);
98  m(0,1) = m(1,0) = precisionMatrix(0,1);
99  m(0,2) = m(2,0) = precisionMatrix(0,2);
100  m(1,1) = precisionMatrix(1,1);
101  m(1,2) = m(2,1) = precisionMatrix(1,2);
102  m(2,2) = precisionMatrix(2,2);
103 
104  m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
105 }
106 
107 /*****************************************************************************/
108 void SpaSolver::getGraph(std::vector<Eigen::Vector2d>& g)
109 /*****************************************************************************/
110 {
111  std::vector<float> raw_graph;
112  m_Spa.getGraph(raw_graph);
113 
114  g.reserve(raw_graph.size()/4);
115 
116  Eigen::Vector2d pose;
117  for (size_t i=0; i!=raw_graph.size()/4; i++)
118  {
119  pose(0) = raw_graph[4*i];
120  pose(1) = raw_graph[4*i+1];
121  g.push_back(pose);
122  }
123 }
124 
125 
126 } // end namespace
std::vector< sba::Node2d, Eigen::aligned_allocator< sba::Node2d > > NodeVector
Definition: spa_solver.cpp:28
int doSPA(int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
Matrix3 Inverse() const
Definition: Karto.h:2543
EdgeLabel * GetLabel()
Definition: Mapper.h:457
void getGraph(std::vector< float > &graph)
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5560
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
const Matrix3 & GetCovariance()
Definition: Mapper.h:219
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Definition: Mapper.h:1026
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
#define forEach(listtype, list)
Definition: Macros.h:66
T * GetObject() const
Definition: Mapper.h:319
kt_int32s GetUniqueId() const
Definition: Karto.h:5161
karto::ScanSolver::IdPoseVector corrections
Definition: spa_solver.hpp:53
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
Definition: spa_solver.cpp:85
virtual void Compute()
Definition: spa_solver.cpp:57
#define ROS_INFO(...)
kt_double GetX() const
Definition: Karto.h:2097
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Definition: spa_solver.cpp:51
Vertex< T > * GetSource() const
Definition: Mapper.h:439
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
Definition: spa_solver.cpp:76
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:953
static Time now()
kt_double GetY() const
Definition: Karto.h:2115
int addNode(const Vector3d &pos, int id)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec)
const Pose2 & GetPoseDifference()
Definition: Mapper.h:210
kt_double GetHeading() const
Definition: Karto.h:2151


slam_toolbox
Author(s): Steve Macenski
autogenerated on Mon Feb 28 2022 23:46:49