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 <ros/console.h>
00019 
00020 #include <nav2d_karto/SpaSolver.h>
00021 
00022 SpaSolver::SpaSolver()
00023 {
00024 //      m_Spa.verbose = true;
00025         mLastSPA = ros::Time::now();
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         typedef std::vector<Node2d, Eigen::aligned_allocator<Node2d> > NodeVector;
00047 
00048         ROS_INFO("Calling doSPA for loop closure");
00049         m_Spa.doSPA(40);
00050         ROS_INFO("Finished doSPA for loop closure");
00051         NodeVector nodes = m_Spa.getNodes();
00052         forEach(NodeVector, &nodes)
00053         {
00054                 karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot);
00055                 corrections.Add(karto::Pair<int, karto::Pose2>(iter->nodeId, pose));
00056         }
00057         mLastSPA = ros::Time::now();
00058 }
00059 
00060 void SpaSolver::reCompute()
00061 {
00062         ros::Duration d = ros::Time::now() - mLastSPA;
00063         if(d.toSec() > 60)
00064                 Compute();
00065 }
00066 
00067 void SpaSolver::AddNode(karto::Vertex<karto::LocalizedObjectPtr>* pVertex)
00068 {
00069         karto::Pose2 pose = pVertex->GetVertexObject()->GetCorrectedPose();
00070         Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading());
00071         m_Spa.addNode(vector, pVertex->GetVertexObject()->GetUniqueId());
00072 }
00073 
00074 void SpaSolver::AddConstraint(karto::Edge<karto::LocalizedObjectPtr>* pEdge)
00075 {
00076         karto::LocalizedObjectPtr pSource = pEdge->GetSource()->GetVertexObject();
00077         karto::LocalizedObjectPtr pTarget = pEdge->GetTarget()->GetVertexObject();
00078         karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel());
00079 
00080         karto::Pose2 diff = pLinkInfo->GetPoseDifference();
00081         Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading());
00082 
00083         karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
00084         Eigen::Matrix<double,3,3> m;
00085         m(0,0) = precisionMatrix(0,0);
00086         m(0,1) = m(1,0) = precisionMatrix(0,1);
00087         m(0,2) = m(2,0) = precisionMatrix(0,2);
00088         m(1,1) = precisionMatrix(1,1);
00089         m(1,2) = m(2,1) = precisionMatrix(1,2);
00090         m(2,2) = precisionMatrix(2,2);
00091 
00092         m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m);
00093 }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 04:05:24