karto: karto: spa_solver.cpp Source 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 "spa_solver.h"
00019 #include <karto/Karto.h>
00020 
00021 #include "ros/console.h"
00022 
00023 SpaSolver::SpaSolver()
00024 {
00025 
00026 }
00027 
00028 SpaSolver::~SpaSolver()
00029 {
00030 
00031 }
00032 
00033 void SpaSolver::Clear()
00034 {
00035   corrections.clear();
00036 }
00037 
00038 const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const
00039 {
00040   return corrections;
00041 }
00042 
00043 void SpaSolver::Compute()
00044 {
00045   corrections.clear();
00046 
00047   typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
00048 
00049   ROS_INFO("Calling doSPA for loop closure");
00050   m_Spa.doSPA(40);
00051   ROS_INFO("Finished doSPA for loop closure");
00052   NodeVector nodes = m_Spa.getNodes();
00053   forEach(NodeVector, &nodes)
00054   {
00055     karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
00056     corrections.push_back(std::make_pair(iter->nodeId, pose));
00057   }
00058 }
00059 
00060 void SpaSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex)
00061 {
00062   karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
00063   Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
00064   m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId());
00065 }
00066 
00067 void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
00068 {
00069   karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject();
00070   karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject();
00071   karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
00072 
00073   karto::Pose2 diff = pLinkInfo->GetPoseDifference();
00074   Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
00075 
00076   karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
00077   Eigen::Matrix<double,3,3> m;
00078   m(0,0) = precisionMatrix(0,0);
00079   m(0,1) = m(1,0) = precisionMatrix(0,1);
00080   m(0,2) = m(2,0) = precisionMatrix(0,2);
00081   m(1,1) = precisionMatrix(1,1);
00082   m(1,2) = m(2,1) = precisionMatrix(1,2);
00083   m(2,2) = precisionMatrix(2,2);
00084 
00085   m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
00086 }