gtsam 4.1.1
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
13namespace gtsam {
14
23class RotateFactor: public NoiseModelFactor1<Rot3> {
24
25 Point3 p_, z_;
26
28 typedef RotateFactor This;
29
30public:
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
74public:
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
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:277
3D rotation represented as a rotation matrix or quaternion
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::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
Definition: Rot3.h:59
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
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
GTSAM_EXPORT void print(const std::string &s=std::string()) const
The print fuction.
Definition: Unit3.cpp:156
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
This is the base class for all factor types.
Definition: Factor.h:56
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:127
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:285
Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Whic...
Definition: RotateFactor.h:23
RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:33
Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:53
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:44
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:39
Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than...
Definition: RotateFactor.h:67
Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:105
RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:77
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:96
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::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:91