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