95 lines
2.3 KiB
C++
95 lines
2.3 KiB
C++
/**
|
|
* @file VSLAMFactor.h
|
|
* @brief A Nonlinear Factor, specialized for visual SLAM
|
|
* @author Alireza Fathi
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "NonlinearFactor.h"
|
|
#include "GaussianFactor.h"
|
|
#include "Cal3_S2.h"
|
|
#include "Testable.h"
|
|
|
|
namespace gtsam {
|
|
|
|
class VSLAMConfig;
|
|
|
|
/**
|
|
* Non-linear factor for a constraint derived from a 2D measurement,
|
|
* i.e. the main building block for visual SLAM.
|
|
*/
|
|
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
|
|
{
|
|
private:
|
|
|
|
int cameraFrameNumber_, landmarkNumber_;
|
|
std::string cameraFrameName_, landmarkName_;
|
|
boost::shared_ptr<Cal3_S2> K_; // Calibration stored in each factor. FD: need to think about this.
|
|
typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
|
|
|
|
public:
|
|
|
|
typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
|
|
typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
|
|
|
|
/**
|
|
* Default constructor
|
|
*/
|
|
VSLAMFactor();
|
|
|
|
/**
|
|
* Constructor
|
|
* @param z is the 2 dimensional location of point in image (the measurement)
|
|
* @param sigma is the standard deviation
|
|
* @param cameraFrameNumber is basically the frame number
|
|
* @param landmarkNumber is the index of the landmark
|
|
* @param K the constant calibration
|
|
*/
|
|
VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const shared_ptrK & K);
|
|
|
|
|
|
/**
|
|
* print
|
|
* @param s optional string naming the factor
|
|
*/
|
|
void print(const std::string& s="VSLAMFactor") const;
|
|
|
|
/**
|
|
* equals
|
|
*/
|
|
bool equals(const VSLAMFactor&, double tol=1e-9) const;
|
|
|
|
/**
|
|
* predict the measurement
|
|
*/
|
|
Vector predict(const VSLAMConfig&) const;
|
|
|
|
/**
|
|
* calculate the error of the factor
|
|
*/
|
|
Vector error_vector(const VSLAMConfig&) const;
|
|
|
|
/**
|
|
* linerarization
|
|
*/
|
|
GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const;
|
|
|
|
int getCameraFrameNumber() const { return cameraFrameNumber_; }
|
|
int getLandmarkNumber() const { return landmarkNumber_; }
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_NVP(cameraFrameNumber_);
|
|
ar & BOOST_SERIALIZATION_NVP(landmarkNumber_);
|
|
ar & BOOST_SERIALIZATION_NVP(cameraFrameName_);
|
|
ar & BOOST_SERIALIZATION_NVP(landmarkName_);
|
|
ar & BOOST_SERIALIZATION_NVP(K_);
|
|
}
|
|
};
|
|
|
|
}
|