Mechanization_bRn2.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/config.h>
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
 
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorConstants.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam_unstable/dllexport.h>
 
#include <list>
 
namespace gtsam {

class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 {
 
private:
  Rot3 bRn_;  
  Vector3 x_g_; 
  Vector3 x_a_; 
 
public:

  Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
      const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) :
      bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
  }

  Mechanization_bRn2(const Mechanization_bRn2& other) = default;
 
  Mechanization_bRn2& operator=(const Mechanization_bRn2& other) = default;

  Vector3 b_g(double g_e) const {
    Vector3 n_g(0, 0, g_e);
    return bRn_ * n_g;
  }
 
  const Rot3& bRn() const {return bRn_; }
  const Vector3& x_g() const { return x_g_; }
  const Vector3& x_a() const { return x_a_; }

  static Mechanization_bRn2 initializeVector(const std::list<Vector>& U,
      const std::list<Vector>& F, const double g_e = 0, bool flat=false);

  static Mechanization_bRn2 initialize(const Matrix& U,
      const Matrix& F, const double g_e = 0, bool flat=false);

  Mechanization_bRn2 correct(const Vector9& dx) const;

  Mechanization_bRn2 integrate(const Vector3& u, const double dt) const;

  void print(const std::string& s = "") const {
    bRn_.print(s + ".R");
 
    std::cout << s + ".x_g" << x_g_ << std::endl;
    std::cout << s + ".x_a" << x_a_ << std::endl;
  }
};
 
} // namespace gtsam
 
#endif  // GTSAM_ALLOW_DEPRECATED_SINCE_V43