SpaSolver.h
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 #ifndef KARTO_SPASOLVER_H
00019 #define KARTO_SPASOLVER_H
00020 
00021 #include "OpenKarto/OpenMapper.h"
00022 
00023 #ifndef EIGEN_USE_NEW_STDVECTOR
00024 #define EIGEN_USE_NEW_STDVECTOR
00025 #endif // EIGEN_USE_NEW_STDVECTOR
00026 
00027 #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)
00028 
00029 #include <Eigen/Eigen>
00030 #include <ros/time.h>
00031 
00032 #include <nav2d_karto/spa2d.h>
00033 
00034 typedef std::vector<karto::Matrix3> CovarianceVector;
00035 
00036 class SpaSolver : public karto::ScanSolver
00037 {
00038 public:
00039         SpaSolver();
00040         virtual ~SpaSolver();
00041 
00042 public:
00043         virtual void Clear();
00044         virtual void Compute();
00045         virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
00046 
00047         virtual void AddNode(karto::Vertex<karto::LocalizedObjectPtr>* pVertex);
00048         virtual void AddConstraint(karto::Edge<karto::LocalizedObjectPtr>* pEdge);
00049 
00050         // Get the underlying graph from SBA
00051         // return the graph of constraints
00053         void getGraph(std::vector<float> &g) { m_Spa.getGraph(g); }
00054         void reCompute();
00055 
00056 private:
00057         karto::ScanSolver::IdPoseVector corrections;
00058 
00059         SysSPA2d m_Spa;
00060         
00061         ros::Time mLastSPA;
00062 };
00063 
00064 #endif // KARTO_SPASOLVER_H
00065 


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