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