gtsam  4.1.0
gtsam
RotateFactor.h
1 /*
2  * @file RotateFactor.cpp
3  * @brief RotateFactor class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
8 #pragma once
9 
11 #include <gtsam/geometry/Rot3.h>
12 
13 namespace gtsam {
14 
23 class RotateFactor: public NoiseModelFactor1<Rot3> {
24 
25  Point3 p_, z_;
26 
28  typedef RotateFactor This;
29 
30 public:
31 
33  RotateFactor(Key key, const Rot3& P, const Rot3& Z,
34  const SharedNoiseModel& model) :
35  Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
36  }
37 
39  gtsam::NonlinearFactor::shared_ptr clone() const override {
40  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
41  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
42 
44  void print(const std::string& s = "",
45  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
46  Base::print(s);
47  std::cout << "RotateFactor:]\n";
48  std::cout << "p: " << p_.transpose() << std::endl;
49  std::cout << "z: " << z_.transpose() << std::endl;
50  }
51 
53  Vector evaluateError(const Rot3& R,
54  boost::optional<Matrix&> H = boost::none) const override {
55  // predict p_ as q = R*z_, derivative H will be filled if not none
56  Point3 q = R.rotate(z_,H);
57  // error is just difference, and note derivative of that wrpt q is I3
58  return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
59  }
60 
61 };
62 
68 
69  Unit3 i_p_, c_z_;
70 
73 
74 public:
75 
77  RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
78  const SharedNoiseModel& model) :
79  Base(model, key), i_p_(i_p), c_z_(c_z) {
80  }
81 
83  static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) {
84  gtsam::Quaternion iRc;
85  // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p
86  iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector());
87  return Rot3(iRc);
88  }
89 
91  gtsam::NonlinearFactor::shared_ptr clone() const override {
92  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
93  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
94 
96  void print(const std::string& s = "",
97  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
98  Base::print(s);
99  std::cout << "RotateDirectionsFactor:" << std::endl;
100  i_p_.print("p");
101  c_z_.print("z");
102  }
103 
105  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
106  Unit3 i_q = iRc * c_z_;
107  Vector error = i_p_.error(i_q, H);
108  if (H) {
109  Matrix DR;
110  iRc.rotate(c_z_, DR);
111  *H = (*H) * DR;
112  }
113  return error;
114  }
115 
117 };
118 } // namespace gtsam
119 
gtsam::RotateFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:44
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition: Rot3M.cpp:149
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::RotateDirectionsFactor::Initialize
static Rot3 Initialize(const Unit3 &i_p, const Unit3 &c_z)
Initialize rotation iRc such that i_p = iRc * c_z.
Definition: RotateFactor.h:83
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::RotateDirectionsFactor
Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than...
Definition: RotateFactor.h:67
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::RotateFactor::RotateFactor
RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:33
gtsam::Unit3::error
GTSAM_EXPORT Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
Signed, vector-valued error between two directions.
Definition: Unit3.cpp:191
gtsam::RotateFactor::evaluateError
Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:53
gtsam::RotateDirectionsFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:91
gtsam::RotateFactor
Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Whic...
Definition: RotateFactor.h:23
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Rot3
Definition: Rot3.h:59
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::Unit3::print
GTSAM_EXPORT void print(const std::string &s=std::string()) const
The print fuction.
Definition: Unit3.cpp:156
gtsam::RotateDirectionsFactor::evaluateError
Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:105
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::Unit3::unitVector
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
gtsam::RotateDirectionsFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:96
gtsam::RotateDirectionsFactor::RotateDirectionsFactor
RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:77
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:119
gtsam::RotateFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:39