GTSAM_solver.hpp

Go to the documentation of this file.

/*********************************************************************
*
*  Copyright (c) 2017, Saurav Agarwal
*  All rights reserved.
*
*********************************************************************/

/* Authors: Saurav Agarwal */
/* Modified: Steve Macenski */

#ifndef KARTO_GTSAMSolver_H
#define KARTO_GTSAMSolver_H

#include <karto_sdk/Mapper.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>

namespace solver_plugins
{

class GTSAMSolver : public karto::ScanSolver
{
  public:

    GTSAMSolver();
    
    virtual ~GTSAMSolver();

  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);

    virtual void getGraph(std::vector<Eigen::Vector2d>& nodes); // std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d> > &edges);

    virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose

  private:
    
    karto::ScanSolver::IdPoseVector corrections_;
    gtsam::NonlinearFactorGraph graph_;
    gtsam::Values initialGuess_;
    std::vector<Eigen::Vector2d> graphNodes_;
};

} // end namespace 

#endif // KARTO_GTSAMSolver_H