#include <GravityFactor.h>
|
| Unit3 | bRef () const |
| |
| virtual gtsam::NonlinearFactor::shared_ptr | clone () const |
| |
| virtual bool | equals (const NonlinearFactor &expected, double tol=1e-9) const |
| |
| virtual Vector | evaluateError (const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const |
| |
| Unit3 | nZ () const |
| |
| | Pose3GravityFactor () |
| |
| | Pose3GravityFactor (Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1)) |
| | Constructor. More...
|
| |
| virtual void | print (const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
| |
| virtual | ~Pose3GravityFactor () |
| |
| Vector | attitudeError (const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const |
| |
| | GravityFactor () |
| |
| | GravityFactor (const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1)) |
| | Constructor. More...
|
| |
| template<class ARCHIVE > |
| void | serialize (ARCHIVE &ar, const unsigned int) |
| |
|
| typedef NoiseModelFactor1< Pose3 > | Base |
| |
|
| template<class ARCHIVE > |
| void | serialize (ARCHIVE &ar, const unsigned int) |
| |
Definition at line 158 of file GravityFactor.h.
| rtabmap::Pose3GravityFactor::Pose3GravityFactor |
( |
| ) |
|
|
inline |
default constructor - only use for serialization
Definition at line 172 of file GravityFactor.h.
| virtual rtabmap::Pose3GravityFactor::~Pose3GravityFactor |
( |
| ) |
|
|
inlinevirtual |
| rtabmap::Pose3GravityFactor::Pose3GravityFactor |
( |
Key |
key, |
|
|
const Unit3 & |
nZ, |
|
|
const SharedNoiseModel & |
model, |
|
|
const Unit3 & |
bRef = Unit3(0, 0, 1) |
|
) |
| |
|
inline |
Constructor.
- Parameters
-
| key | of the Pose3 variable that will be constrained |
| nZ | measured direction in navigation frame (remove yaw before rotating the gravity vector) |
| model | Gaussian noise model |
| bRef | reference direction in body frame (default Z-axis) |
Definition at line 185 of file GravityFactor.h.
| Unit3 rtabmap::Pose3GravityFactor::bRef |
( |
| ) |
const |
|
inline |
| virtual gtsam::NonlinearFactor::shared_ptr rtabmap::Pose3GravityFactor::clone |
( |
| ) |
const |
|
inlinevirtual |
| bool rtabmap::Pose3GravityFactor::equals |
( |
const NonlinearFactor & |
expected, |
|
|
double |
tol = 1e-9 |
|
) |
| const |
|
virtual |
| virtual Vector rtabmap::Pose3GravityFactor::evaluateError |
( |
const Pose3 & |
nTb, |
|
|
boost::optional< Matrix & > |
H = boost::none |
|
) |
| const |
|
inlinevirtual |
| Unit3 rtabmap::Pose3GravityFactor::nZ |
( |
| ) |
const |
|
inline |
| void rtabmap::Pose3GravityFactor::print |
( |
const std::string & |
s, |
|
|
const KeyFormatter & |
keyFormatter = DefaultKeyFormatter |
|
) |
| const |
|
virtual |
template<class ARCHIVE >
| void rtabmap::Pose3GravityFactor::serialize |
( |
ARCHIVE & |
ar, |
|
|
const unsigned |
int |
|
) |
| |
|
inlineprivate |
| friend class boost::serialization::access |
|
friend |
The documentation for this class was generated from the following files: