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 "SpaSolver.h"
19 #include <OpenKarto/Karto.h>
20 
22 {
23 
24 }
25 
27 {
28 
29 }
30 
32 {
33  corrections.clear();
34 }
35 
37 {
38  return corrections;
39 }
40 
42 {
43  corrections.clear();
44 
45  typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
46 
47  printf("DO SPA BEGIN\n");
48  m_Spa.doSPA(40);
49  printf("DO SPA END\n");
50  NodeVector nodes = m_Spa.getNodes();
51  forEach(NodeVector, &nodes)
52  {
53  karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
54  corrections.push_back(std::make_pair(iter->nodeId, pose));
55  }
56 }
57 
59 {
60  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
61  Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
62  m_Spa.addNode2d(vector, pVertex->GetObject()->GetUniqueId());
63 }
64 
66 {
67  karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
68  karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
69  karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
70 
71  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
72  Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
73 
74  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
75  Eigen::Matrix<double,3,3> m;
76  m(0,0) = precisionMatrix(0,0);
77  m(0,1) = m(1,0) = precisionMatrix(0,1);
78  m(0,2) = m(2,0) = precisionMatrix(0,2);
79  m(1,1) = precisionMatrix(1,1);
80  m(1,2) = m(2,1) = precisionMatrix(1,2);
81  m(2,2) = precisionMatrix(2,2);
82 
83  m_Spa.addConstraint2d(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
84 }
EdgeLabel * GetLabel()
Definition: Mapper.h:350
kt_double GetHeading() const
Definition: Karto.h:2064
T * GetObject() const
Definition: Mapper.h:241
virtual const karto::ScanSolver::IdPoseVector & GetCorrections() const
Definition: SpaSolver.cpp:36
kt_double GetX() const
Definition: Karto.h:2010
virtual void AddNode(karto::Vertex< karto::LocalizedRangeScan > *pVertex)
Definition: SpaSolver.cpp:58
const Matrix3 & GetCovariance()
Definition: Mapper.h:184
#define forEach(listtype, list)
Definition: Macros.h:66
sba::SysSPA2d m_Spa
Definition: SpaSolver.h:51
kt_int32s GetUniqueId() const
Definition: Karto.h:4847
virtual void Clear()
Definition: SpaSolver.cpp:31
karto::ScanSolver::IdPoseVector corrections
Definition: SpaSolver.h:49
virtual void Compute()
Definition: SpaSolver.cpp:41
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:724
kt_double GetY() const
Definition: Karto.h:2028
virtual void AddConstraint(karto::Edge< karto::LocalizedRangeScan > *pEdge)
Definition: SpaSolver.cpp:65
virtual ~SpaSolver()
Definition: SpaSolver.cpp:26
Vertex< T > * GetSource() const
Definition: Mapper.h:332
Vertex< T > * GetTarget() const
Definition: Mapper.h:341
Matrix3 Inverse() const
Definition: Karto.h:2448
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5211
const Pose2 & GetPoseDifference()
Definition: Mapper.h:175


open_karto
Author(s):
autogenerated on Mon Jun 10 2019 14:02:19