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.h"
19 #include <open_karto/Karto.h>
20 
21 #include "ros/console.h"
22 
24 {
25 
26 }
27 
29 {
30 
31 }
32 
33 void SpaSolver::Clear()
34 {
35  corrections.clear();
36 }
37 
39 {
40  return corrections;
41 }
42 
43 void SpaSolver::Compute()
44 {
45  corrections.clear();
46 
47  typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
48 
49  ROS_INFO("Calling doSPA for loop closure");
50  m_Spa.doSPA(40);
51  ROS_INFO("Finished doSPA for loop closure");
52  NodeVector nodes = m_Spa.getNodes();
53  forEach(NodeVector, &nodes)
54  {
55  karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
56  corrections.push_back(std::make_pair(iter->nodeId, pose));
57  }
58 }
59 
61 {
62  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
63  Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
64  m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
65 }
66 
68 {
69  karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
70  karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
71  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
72 
73  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
74  Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
75 
76  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
77  Eigen::Matrix<double,3,3> m;
78  m(0,0) = precisionMatrix(0,0);
79  m(0,1) = m(1,0) = precisionMatrix(0,1);
80  m(0,2) = m(2,0) = precisionMatrix(0,2);
81  m(1,1) = precisionMatrix(1,1);
82  m(1,2) = m(2,1) = precisionMatrix(1,2);
83  m(2,2) = precisionMatrix(2,2);
84 
85  m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
86 }
int doSPA(int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
EdgeLabel * GetLabel()
kt_double GetHeading() const
T * GetObject() const
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Definition: spa_solver.cpp:38
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
kt_double GetX() const
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
Definition: spa_solver.cpp:60
const Matrix3 & GetCovariance()
#define forEach(listtype, list)
sba::SysSPA2d m_Spa
Definition: spa_solver.h:56
#define ROS_INFO(...)
kt_int32s GetUniqueId() const
virtual void Clear()
Definition: spa_solver.cpp:33
karto::ScanSolver::IdPoseVector corrections
Definition: spa_solver.h:54
virtual void Compute()
Definition: spa_solver.cpp:43
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
kt_double GetY() const
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
Definition: spa_solver.cpp:67
virtual ~SpaSolver()
Definition: spa_solver.cpp:28
Vertex< T > * GetSource() const
int addNode(const Vector3d &pos, int id)
Vertex< T > * GetTarget() const
Matrix3 Inverse() const
bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec)
const Pose2 & GetCorrectedPose() const
const Pose2 & GetPoseDifference()


slam_karto
Author(s): Brian Gerkey
autogenerated on Tue Apr 2 2019 02:59:33