12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455 |
- #ifndef __SPASOLVER__
- #define __SPASOLVER__
- #include <open_karto/Mapper.h>
- #ifndef EIGEN_USE_NEW_STDVECTOR
- #define EIGEN_USE_NEW_STDVECTOR
- #endif
- #define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(10)
- #include <Eigen/Eigen>
- #include "sparse_bundle_adjustment/spa2d.h"
- typedef std::vector<karto::Matrix3> CovarianceVector;
- class SpaSolver : public karto::ScanSolver
- {
- public:
- SpaSolver();
- virtual ~SpaSolver();
- public:
- virtual void Clear();
- virtual void Compute();
- virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
- virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex);
- virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge);
- private:
- karto::ScanSolver::IdPoseVector corrections;
- sba::SysSPA2d m_Spa;
- };
- #endif
|