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 <open_karto/Mapper.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 #include <Eigen/Eigen> 00029 00030 #include <sparse_bundle_adjustment/spa2d.h> 00031 00032 typedef std::vector<karto::Matrix3> CovarianceVector; 00033 00034 class SpaSolver : public karto::ScanSolver 00035 { 00036 public: 00037 SpaSolver(); 00038 virtual ~SpaSolver(); 00039 00040 public: 00041 virtual void Clear(); 00042 virtual void Compute(); 00043 virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; 00044 00045 virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex); 00046 virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge); 00047 00048 // Get the underlying graph from SBA 00049 // return the graph of constraints 00051 void getGraph(std::vector<float> &g) { m_Spa.getGraph(g); } 00052 00053 private: 00054 karto::ScanSolver::IdPoseVector corrections; 00055 00056 sba::SysSPA2d m_Spa; 00057 }; 00058 00059 #endif // KARTO_SPASOLVER_H 00060