mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 17:17:47 +08:00
matlabbeandFrank Dellaert
0156bc22ed
* Fixing 4.3.1-ros gtsam compatibility * javobian fix in SwitchVariable * Fix GTSAM scalar switch traits, Jacobians and attitude API detection * Separate sigmoid factor correction from GTSAM compatibility * Correct sigmoid switch Jacobian and add focused factor regression * combined gtsam tests in same file * cmake refactor --------- Co-authored-by: Frank Dellaert <[email protected]>
127 lines
3.7 KiB
C++
127 lines
3.7 KiB
C++
/*
|
|
* switchVariableLinear.h
|
|
*
|
|
* Created on: 02.08.2012
|
|
* Author: niko
|
|
*/
|
|
|
|
#ifndef SWITCHVARIABLELINEAR_H_
|
|
#define SWITCHVARIABLELINEAR_H_
|
|
|
|
#pragma once
|
|
|
|
// DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
|
|
#include "DerivedValue.h"
|
|
#include <gtsam/base/Lie.h>
|
|
#include <gtsam/base/Manifold.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
|
|
namespace vertigo {
|
|
|
|
/**
|
|
* SwitchVariableLinear is a wrapper around double to allow it to be a Lie type
|
|
*/
|
|
struct SwitchVariableLinear : public rtabmap::DerivedValue<SwitchVariableLinear> {
|
|
|
|
/** default constructor */
|
|
SwitchVariableLinear() : d_(0.0) {};
|
|
|
|
/** wrap a double */
|
|
SwitchVariableLinear(double d) : d_(d) {
|
|
// if (d_ < 0.0) d_=0.0;
|
|
// else if(d_>1.0) d_=1.0;
|
|
};
|
|
|
|
/** access the underlying value */
|
|
double value() const { return d_; }
|
|
|
|
/** print @param s optional string naming the object */
|
|
inline void print(const std::string& name="") const {
|
|
std::cout << name << ": " << d_ << std::endl;
|
|
}
|
|
|
|
/** equality up to tolerance */
|
|
inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
|
|
return fabs(expected.d_ - d_) <= tol;
|
|
}
|
|
|
|
// Manifold requirements
|
|
static constexpr int dimension = 1;
|
|
|
|
/** Returns dimensionality of the tangent space */
|
|
inline size_t dim() const { return 1; }
|
|
inline static size_t Dim() { return 1; }
|
|
|
|
/** Update the SwitchVariableLinear with a tangent space update */
|
|
inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
|
|
double x = value() + v(0);
|
|
|
|
if (x>1.0) x=1.0;
|
|
else if (x<0.0) x=0.0;
|
|
|
|
return SwitchVariableLinear(x);
|
|
}
|
|
|
|
/** @return the local coordinates of another object */
|
|
inline gtsam::Vector1 localCoordinates(const SwitchVariableLinear& t2,
|
|
gtsam::OptionalJacobian<1, 1> H1 = {},
|
|
gtsam::OptionalJacobian<1, 1> H2 = {}) const {
|
|
if (H1) *H1 = -gtsam::Matrix11::Identity();
|
|
if (H2) *H2 = gtsam::Matrix11::Identity();
|
|
return gtsam::Vector1(t2.value() - value());
|
|
}
|
|
|
|
// Group requirements
|
|
|
|
/** identity */
|
|
inline static SwitchVariableLinear identity() {
|
|
return SwitchVariableLinear();
|
|
}
|
|
|
|
/** compose with another object */
|
|
inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const {
|
|
return SwitchVariableLinear(d_ + p.d_);
|
|
}
|
|
|
|
/** between operation */
|
|
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
|
|
#if GTSAM_VERSION_NUMERIC >= 40300
|
|
gtsam::OptionalMatrixType H1=OptionalNone,
|
|
gtsam::OptionalMatrixType H2=OptionalNone) const {
|
|
#else
|
|
boost::optional<gtsam::Matrix&> H1=boost::none,
|
|
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
|
#endif
|
|
if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
|
|
if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
|
|
return SwitchVariableLinear(l2.value() - value());
|
|
}
|
|
|
|
/** invert the object and yield a new one */
|
|
inline SwitchVariableLinear inverse() const {
|
|
return SwitchVariableLinear(-1.0 * value());
|
|
}
|
|
|
|
// Lie functions
|
|
|
|
/** Expmap around identity */
|
|
static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
|
|
|
|
/** Logmap around identity - just returns with default cast back */
|
|
static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
|
|
|
|
private:
|
|
double d_;
|
|
};
|
|
}
|
|
|
|
namespace gtsam {
|
|
// Use the scalar manifold's dimension, category and chart operations.
|
|
template<> struct traits<vertigo::SwitchVariableLinear>
|
|
: internal::Manifold<vertigo::SwitchVariableLinear> {};
|
|
}
|
|
|
|
|
|
|
|
#endif /* SWITCHVARIABLELINEAR_H_ */
|