Removed superfluous size, added doxygen partitions
parent
458a33dade
commit
e22c24eff5
|
@ -296,6 +296,8 @@ protected:
|
||||||
typedef NoiseModelFactor1<VALUE> This;
|
typedef NoiseModelFactor1<VALUE> This;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
/** Default constructor for I/O only */
|
/** Default constructor for I/O only */
|
||||||
NoiseModelFactor1() {}
|
NoiseModelFactor1() {}
|
||||||
|
@ -309,13 +311,20 @@ public:
|
||||||
* @param noiseModel shared pointer to noise model
|
* @param noiseModel shared pointer to noise model
|
||||||
* @param key1 by which to look up X value in Values
|
* @param key1 by which to look up X value in Values
|
||||||
*/
|
*/
|
||||||
NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
|
NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1)
|
||||||
Base(noiseModel, cref_list_of<1>(key1)) {}
|
: Base(noiseModel, cref_list_of<1>(key1)) {}
|
||||||
|
|
||||||
/** Calls the 1-key specific version of evaluateError, which is pure virtual
|
/// @}
|
||||||
* so must be implemented in the derived class.
|
/// @name NoiseModelFactor methods
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calls the 1-key specific version of evaluateError below, which is pure
|
||||||
|
* virtual so must be implemented in the derived class.
|
||||||
*/
|
*/
|
||||||
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
|
Vector unwhitenedError(
|
||||||
|
const Values &x,
|
||||||
|
boost::optional<std::vector<Matrix> &> H = boost::none) const override {
|
||||||
if (this->active(x)) {
|
if (this->active(x)) {
|
||||||
const X &x1 = x.at<X>(keys_[0]);
|
const X &x1 = x.at<X>(keys_[0]);
|
||||||
if (H) {
|
if (H) {
|
||||||
|
@ -328,16 +337,22 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Virtual methods
|
||||||
|
/// @{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Override this method to finish implementing a unary factor.
|
* Override this method to finish implementing a unary factor.
|
||||||
* If the optional Matrix reference argument is specified, it should compute
|
* If the optional Matrix reference argument is specified, it should compute
|
||||||
* both the function evaluation and its derivative in X.
|
* both the function evaluation and its derivative in X.
|
||||||
*/
|
*/
|
||||||
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
|
virtual Vector
|
||||||
boost::none) const = 0;
|
evaluateError(const X &x,
|
||||||
|
boost::optional<Matrix &> H = boost::none) const = 0;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
|
|
|
@ -67,9 +67,11 @@ namespace gtsam {
|
||||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||||
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||||
|
|
||||||
/** implement functions needed for Testable */
|
/// @}
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
/** print */
|
/// print with optional string
|
||||||
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||||
std::cout << s << "BetweenFactor("
|
std::cout << s << "BetweenFactor("
|
||||||
<< keyFormatter(this->key1()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
|
@ -78,15 +80,17 @@ namespace gtsam {
|
||||||
this->noiseModel_->print(" noise model: ");
|
this->noiseModel_->print(" noise model: ");
|
||||||
}
|
}
|
||||||
|
|
||||||
/** equals */
|
/// assert equality up to a tolerance
|
||||||
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
|
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
|
||||||
const This *e = dynamic_cast<const This*> (&expected);
|
const This *e = dynamic_cast<const This*> (&expected);
|
||||||
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
|
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** implement functions needed to derive from Factor */
|
/// @}
|
||||||
|
/// @name NoiseModelFactor2 methods
|
||||||
|
/// @{
|
||||||
|
|
||||||
/** vector of errors */
|
/// evaluate error, returns vector of errors size of tangent space
|
||||||
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
|
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
|
||||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
|
boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
|
||||||
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
|
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
|
||||||
|
@ -102,15 +106,15 @@ namespace gtsam {
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/** return the measured */
|
/// @}
|
||||||
|
/// @name Standard interface
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// return the measurement
|
||||||
const VALUE& measured() const {
|
const VALUE& measured() const {
|
||||||
return measured_;
|
return measured_;
|
||||||
}
|
}
|
||||||
|
/// @}
|
||||||
/** number of variables attached to this factor */
|
|
||||||
std::size_t size() const {
|
|
||||||
return 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
|
@ -87,11 +87,6 @@ public:
|
||||||
return measuredE_;
|
return measuredE_;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** number of variables attached to this factor */
|
|
||||||
std::size_t size() const {
|
|
||||||
return 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
|
|
|
@ -403,11 +403,6 @@ public:
|
||||||
return measured_;
|
return measured_;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** number of variables attached to this factor */
|
|
||||||
std::size_t size() const {
|
|
||||||
return 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t dim() const override {
|
size_t dim() const override {
|
||||||
return model_inlier_->R().rows() + model_inlier_->R().cols();
|
return model_inlier_->R().rows() + model_inlier_->R().cols();
|
||||||
}
|
}
|
||||||
|
|
|
@ -203,11 +203,6 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
/** number of variables attached to this factor */
|
|
||||||
std::size_t size() const {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t dim() const override {
|
size_t dim() const override {
|
||||||
return model_->R().rows() + model_->R().cols();
|
return model_->R().rows() + model_->R().cols();
|
||||||
}
|
}
|
||||||
|
|
|
@ -401,11 +401,6 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
/** number of variables attached to this factor */
|
|
||||||
std::size_t size() const {
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t dim() const override {
|
size_t dim() const override {
|
||||||
return model_inlier_->R().rows() + model_inlier_->R().cols();
|
return model_inlier_->R().rows() + model_inlier_->R().cols();
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue