67 lines
1.4 KiB
C++
67 lines
1.4 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 <gtsam/NonlinearFactorGraph.h>
|
|
#include <gtsam/FactorGraph-inl.h>
|
|
#include "VSLAMFactor.h"
|
|
#include "VSLAMFactor0.h"
|
|
#include "StereoFactor.h"
|
|
#include "VSLAMConfig.h"
|
|
|
|
|
|
using namespace std;
|
|
|
|
/**
|
|
* Non-linear factor graph for visual SLAM
|
|
*/
|
|
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>{
|
|
private:
|
|
int nFrames;
|
|
typedef map <int, int> feat_ids_type;
|
|
feat_ids_type feat_ids;
|
|
|
|
public:
|
|
|
|
/** default constructor is empty graph */
|
|
VSLAMGraph() {}
|
|
|
|
/**
|
|
* Constructor that loads measurements from file
|
|
* @param path to the file
|
|
*/
|
|
VSLAMGraph(const std::string& path);
|
|
|
|
/**
|
|
* Constructor that loads from VO file (not tested)
|
|
* @param path to the file
|
|
* @param nrFrames the number of frames to load
|
|
* @return new factor graph
|
|
*/
|
|
VSLAMGraph(const std::string& path, int nrFrames, double sigma, const gtsam::Cal3_S2& K);
|
|
|
|
/**
|
|
* print out graph
|
|
*/
|
|
void print(const std::string& s = "") const {
|
|
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
|
|
}
|
|
|
|
void load_dumped(const std::string& path);
|
|
|
|
int Get_nFrames(){return nFrames;};
|
|
int Get_nFeat_ids(){return feat_ids.size();};
|
|
feat_ids_type* Get_feat_ids_map(){return &feat_ids;};
|
|
};
|