Mechanization_bRn2.h
Go to the documentation of this file.
1 
8 #pragma once
9 
10 #include <gtsam/geometry/Rot3.h>
11 #include <gtsam/base/Vector.h>
12 #include <gtsam_unstable/dllexport.h>
13 #include <list>
14 
15 namespace gtsam {
16 
17 class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 {
18 
19 private:
23 
24 public:
25 
32  Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
33  const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) :
34  bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
35  }
36 
39  bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) {
40  }
41 
43  Vector3 b_g(double g_e) const {
44  Vector3 n_g(0, 0, g_e);
45  return bRn_ * n_g;
46  }
47 
48  const Rot3& bRn() const {return bRn_; }
49  const Vector3& x_g() const { return x_g_; }
50  const Vector3& x_a() const { return x_a_; }
51 
59  static Mechanization_bRn2 initializeVector(const std::list<Vector>& U,
60  const std::list<Vector>& F, const double g_e = 0, bool flat=false);
61 
63  static Mechanization_bRn2 initialize(const Matrix& U,
64  const Matrix& F, const double g_e = 0, bool flat=false);
65 
71  Mechanization_bRn2 correct(const Vector9& dx) const;
72 
79  Mechanization_bRn2 integrate(const Vector3& u, const double dt) const;
80 
82  void print(const std::string& s = "") const {
83  bRn_.print(s + ".R");
84 
85  std::cout << s + ".x_g" << x_g_ << std::endl;
86  std::cout << s + ".x_a" << x_a_ << std::endl;
87  }
88 
89 };
90 
91 } // namespace gtsam
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 x_a_
accelerometer bias
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Rot3 bRn_
rotation from nav to body
const double dt
double g_e
Definition: testAHRS.cpp:21
Mechanization_bRn2(const Rot3 &initial_bRn=Rot3(), const Vector3 &initial_x_g=Z_3x1, const Vector3 &initial_x_a=Z_3x1)
const Vector3 & x_a() const
Signature::Row F
Definition: Signature.cpp:53
const Rot3 & bRn() const
void print(const std::string &s="") const
Definition: Rot3.cpp:34
RealScalar s
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
const Vector3 & x_g() const
Vector3 x_g_
gyroscope bias
Mechanization_bRn2(const Mechanization_bRn2 &other)
Copy constructor.
void print(const std::string &s="") const
print
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:338
Vector3 b_g(double g_e) const
gravity in the body frame
3D rotation represented as a rotation matrix or quaternion


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:59