SpaSolver.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #include "SpaSolver.h"
00019 #include <OpenKarto/Karto.h>
00020 
00021 SpaSolver::SpaSolver()
00022 {
00023 
00024 }
00025 
00026 SpaSolver::~SpaSolver()
00027 {
00028 
00029 }
00030 
00031 void SpaSolver::Clear()
00032 {
00033   corrections.clear();
00034 }
00035 
00036 const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const
00037 {
00038   return corrections;
00039 }
00040 
00041 void SpaSolver::Compute()
00042 {
00043   corrections.clear();
00044 
00045   typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
00046 
00047   printf("DO SPA BEGIN\n");
00048   m_Spa.doSPA(40);
00049   printf("DO SPA END\n");
00050   NodeVector nodes = m_Spa.getNodes();
00051   forEach(NodeVector, &nodes)
00052   {
00053     karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
00054     corrections.push_back(std::make_pair(iter->nodeId, pose));
00055   }
00056 }
00057 
00058 void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
00059 {
00060   karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
00061   Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
00062   m_Spa.addNode2d(vector, pVertex->GetObject()->GetUniqueId());
00063 }
00064 
00065 void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
00066 {
00067   karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
00068   karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
00069   karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
00070 
00071   karto::Pose2 diff = pLinkInfo->GetPoseDifference();
00072   Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
00073 
00074   karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
00075   Eigen::Matrix<double,3,3> m;
00076   m(0,0) = precisionMatrix(0,0);
00077   m(0,1) = m(1,0) = precisionMatrix(0,1);
00078   m(0,2) = m(2,0) = precisionMatrix(0,2);
00079   m(1,1) = precisionMatrix(1,1);
00080   m(1,2) = m(2,1) = precisionMatrix(1,2);
00081   m(2,2) = precisionMatrix(2,2);
00082 
00083   m_Spa.addConstraint2d(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
00084 }


open_karto
Author(s):
autogenerated on Tue May 2 2017 02:41:15