GravityFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
26 #include "GravityFactor.h"
27 
28 using namespace std;
29 
30 namespace rtabmap {
31 
32 //***************************************************************************
33 Vector GravityFactor::attitudeError(const Rot3& nRb,
34  OptionalJacobian<2, 3> H) const {
35  if (H) {
36  Matrix23 D_nRef_R;
37  Matrix22 D_e_nRef;
38  Vector3 r = nRb.xyz();
39  Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
40  Vector e = nZ_.error(nRef, D_e_nRef);
41  (*H) = D_e_nRef * D_nRef_R;
42  //printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
43  // nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
44  // (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
45  return e;
46  } else {
47  Vector3 r = nRb.xyz();
48  Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
49  Vector e = nZ_.error(nRef);
50  //printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
51  return e;
52  }
53 }
54 
55 //***************************************************************************
56 void Rot3GravityFactor::print(const string& s,
57  const KeyFormatter& keyFormatter) const {
58  cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
59  nZ_.print(" measured direction in nav frame: ");
60  bRef_.print(" reference direction in body frame: ");
61  this->noiseModel_->print(" noise model: ");
62 }
63 
64 //***************************************************************************
65 bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
66  double tol) const {
67  const This* e = dynamic_cast<const This*>(&expected);
68  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
69  && this->bRef_.equals(e->bRef_, tol);
70 }
71 
72 //***************************************************************************
73 void Pose3GravityFactor::print(const string& s,
74  const KeyFormatter& keyFormatter) const {
75  cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
76  nZ_.print(" measured direction in nav frame: ");
77  bRef_.print(" reference direction in body frame: ");
78  this->noiseModel_->print(" noise model: ");
79 }
80 
81 //***************************************************************************
82 bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
83  double tol) const {
84  const This* e = dynamic_cast<const This*>(&expected);
85  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
86  && this->bRef_.equals(e->bRef_, tol);
87 }
88 
89 //***************************************************************************
90 
91 }
#define NULL
GLM_FUNC_DECL genType e()
const Unit3 bRef_
Position measurement in.
Definition: GravityFactor.h:47
PM::Vector Vector
GLM_FUNC_DECL detail::tmat4x4< T, P > rotate(detail::tmat4x4< T, P > const &m, T const &angle, detail::tvec3< T, P > const &axis)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28