SpaSolver.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 <ros/console.h>
19 
20 #include <nav2d_karto/SpaSolver.h>
21 
23 {
24 // m_Spa.verbose = true;
26 }
27 
29 {
30 
31 }
32 
34 {
36 }
37 
39 {
40  return corrections;
41 }
42 
44 {
46  typedef std::vector<Node2d, Eigen::aligned_allocator<Node2d> > NodeVector;
47 
48  ROS_INFO("Calling doSPA for loop closure");
49  m_Spa.doSPA(40);
50  ROS_INFO("Finished doSPA for loop closure");
51  NodeVector nodes = m_Spa.getNodes();
52  forEach(NodeVector, &nodes)
53  {
54  karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
55  corrections.Add(karto::Pair<int, karto::Pose2>(iter->nodeId, pose));
56  }
58 }
59 
61 {
63  if(d.toSec() > 60)
64  Compute();
65 }
66 
68 {
69  karto::Pose2 pose = pVertex->GetVertexObject()->GetCorrectedPose();
70  Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
71  m_Spa.addNode(vector, pVertex->GetVertexObject()->GetUniqueId());
72 }
73 
75 {
76  karto::LocalizedObjectPtr pSource = pEdge->GetSource()->GetVertexObject();
77  karto::LocalizedObjectPtr pTarget = pEdge->GetTarget()->GetVertexObject();
78  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
79 
80  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
81  Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
82 
83  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
84  Eigen::Matrix<double,3,3> m;
85  m(0,0) = precisionMatrix(0,0);
86  m(0,1) = m(1,0) = precisionMatrix(0,1);
87  m(0,2) = m(2,0) = precisionMatrix(0,2);
88  m(1,1) = precisionMatrix(1,1);
89  m(1,2) = m(2,1) = precisionMatrix(1,2);
90  m(2,2) = precisionMatrix(2,2);
91 
92  m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
93 }
d
EdgeLabel * GetLabel()
Definition: OpenMapper.h:360
kt_double GetHeading() const
Definition: Geometry.h:2274
#define forEach(listtype, list)
Definition: Macros.h:88
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Definition: SpaSolver.cpp:38
virtual void AddConstraint(karto::Edge< karto::LocalizedObjectPtr > *pEdge)
Definition: SpaSolver.cpp:74
T GetVertexObject() const
Definition: OpenMapper.h:251
void reCompute()
Definition: SpaSolver.cpp:60
kt_double GetX() const
Definition: Geometry.h:2220
const Matrix3 & GetCovariance()
Definition: OpenMapper.h:194
virtual void Add(const T &rValue)
Definition: List.h:111
SysSPA2d m_Spa
Definition: SpaSolver.h:59
bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec)
Definition: spa2d.cpp:231
#define ROS_INFO(...)
virtual void Clear()
Definition: SpaSolver.cpp:33
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
Definition: spa2d.h:206
Vector3< kt_double > Vector3d
Definition: Geometry.h:1046
int addNode(const Vector3d &pos, int id)
Definition: spa2d.cpp:208
karto::ScanSolver::IdPoseVector corrections
Definition: SpaSolver.h:57
virtual void Compute()
Definition: SpaSolver.cpp:43
virtual void AddNode(karto::Vertex< karto::LocalizedObjectPtr > *pVertex)
Definition: SpaSolver.cpp:67
int doSPA(int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
Definition: spa2d.cpp:434
static Time now()
kt_double GetY() const
Definition: Geometry.h:2238
virtual ~SpaSolver()
Definition: SpaSolver.cpp:28
Vertex< T > * GetSource() const
Definition: OpenMapper.h:342
virtual void Clear()
Definition: List.h:231
Vertex< T > * GetTarget() const
Definition: OpenMapper.h:351
Matrix3 Inverse() const
Definition: Geometry.h:2659
ros::Time mLastSPA
Definition: SpaSolver.h:61
const Pose2 & GetPoseDifference()
Definition: OpenMapper.h:185


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24