69 lines
1.5 KiB
C++
69 lines
1.5 KiB
C++
/**
|
|
* @file VSLAMGraph.h
|
|
* @brief A factor graph for the VSLAM problem
|
|
* @author Alireza Fathi
|
|
* @author Carlos Nieto
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <vector>
|
|
#include <map>
|
|
#include <set>
|
|
#include <fstream>
|
|
|
|
#include "VSLAMFactor.h"
|
|
#include "NonlinearFactorGraph.h"
|
|
#include "FactorGraph-inl.h"
|
|
#include "VSLAMConfig.h"
|
|
#include "Testable.h"
|
|
|
|
namespace gtsam{
|
|
|
|
/**
|
|
* Non-linear factor graph for visual SLAM
|
|
*/
|
|
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>{
|
|
|
|
public:
|
|
|
|
/** default constructor is empty graph */
|
|
VSLAMGraph() {}
|
|
|
|
/**
|
|
* print out graph
|
|
*/
|
|
void print(const std::string& s = "") const {
|
|
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
|
|
}
|
|
|
|
/**
|
|
* equals
|
|
*/
|
|
bool equals(const VSLAMGraph& p, double tol=1e-9) const {
|
|
return gtsam::NonlinearFactorGraph<VSLAMConfig>::equals(p, tol);
|
|
}
|
|
|
|
/**
|
|
* Add a constraint on a landmark (for now, *must* be satisfied in any Config)
|
|
* @param j index of landmark
|
|
* @param p to which point to constrain it to
|
|
*/
|
|
void addLandmarkConstraint(int j, const Point3& p = Point3());
|
|
|
|
/**
|
|
* Add a constraint on a camera (for now, *must* be satisfied in any Config)
|
|
* @param j index of camera
|
|
* @param p to which pose to constrain it to
|
|
*/
|
|
void addCameraConstraint(int j, const Pose3& p = Pose3());
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int version) {}
|
|
};
|
|
|
|
} // namespace gtsam
|