90 lines
2.2 KiB
C++
90 lines
2.2 KiB
C++
/**
|
|
* @file VSLAMConfig.h
|
|
* @brief Config for VSLAM
|
|
* @author Alireza Fathi
|
|
* @author Carlos Nieto
|
|
*/
|
|
|
|
#include <string>
|
|
#include <map>
|
|
#include <vector>
|
|
#include <fstream>
|
|
|
|
#include "Pose3Config.h"
|
|
|
|
#pragma once
|
|
|
|
namespace gtsam{
|
|
|
|
/**
|
|
* Config that knows about points and poses
|
|
*/
|
|
class VSLAMConfig : Testable<VSLAMConfig> {
|
|
|
|
public:
|
|
|
|
typedef Symbol<Pose3,'x'> PoseKey;
|
|
typedef Symbol<Point3,'l'> PointKey;
|
|
|
|
private:
|
|
|
|
LieConfig<PoseKey, Pose3> poses_;
|
|
LieConfig<PointKey, Point3> points_;
|
|
|
|
public:
|
|
|
|
/**
|
|
* default constructor
|
|
*/
|
|
VSLAMConfig() {}
|
|
|
|
/**
|
|
* print
|
|
*/
|
|
void print(const std::string& s = "") const;
|
|
|
|
/**
|
|
* check whether two configs are equal
|
|
*/
|
|
bool equals(const VSLAMConfig& c, double tol=1e-6) const;
|
|
|
|
/**
|
|
* Get Poses or Points
|
|
*/
|
|
inline const Pose3& operator[](const PoseKey& key) const {return poses_[key];}
|
|
inline const Point3& operator[](const PointKey& key) const {return points_[key];}
|
|
|
|
// (Awkwardly named) backwards compatibility:
|
|
|
|
inline bool cameraPoseExists (const PoseKey& key) const {return poses_.exists(key);}
|
|
inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);}
|
|
|
|
inline Pose3 cameraPose (const PoseKey& key) const {return poses_[key];}
|
|
inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];}
|
|
|
|
inline size_t size() const {return points_.size() + poses_.size();}
|
|
inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);}
|
|
|
|
// Imperative functions:
|
|
|
|
inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);}
|
|
inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);}
|
|
|
|
inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);}
|
|
inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);}
|
|
|
|
inline void clear() {points_.clear(); poses_.clear();}
|
|
|
|
friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
|
|
};
|
|
|
|
|
|
/**
|
|
* Exponential map: takes 6D vectors in VectorConfig
|
|
* and applies them to the poses in the VSLAMConfig.
|
|
* Needed for use in nonlinear optimization
|
|
*/
|
|
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
|
|
} // namespace gtsam
|
|
|