RotateFactor.h
Go to the documentation of this file.
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 NoiseModelFactorN<Rot3> {
24 
26 
28  typedef RotateFactor This;
29 
30 public:
31 
32  // Provide access to the Matrix& version of evaluateError:
33  using Base::evaluateError;
34 
36  RotateFactor(Key key, const Rot3& P, const Rot3& Z,
37  const SharedNoiseModel& model) :
38  Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
39  }
40 
43  return std::static_pointer_cast<gtsam::NonlinearFactor>(
45 
47  void print(const std::string& s = "",
48  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
49  Base::print(s);
50  std::cout << "RotateFactor:]\n";
51  std::cout << "p: " << p_.transpose() << std::endl;
52  std::cout << "z: " << z_.transpose() << std::endl;
53  }
54 
56  Vector evaluateError(const Rot3& R, OptionalMatrixType H) const override {
57  // predict p_ as q = R*z_, derivative H will be filled if not none
58  Point3 q = R.rotate(z_,H);
59  // error is just difference, and note derivative of that wrpt q is I3
60  return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
61  }
62 
63 };
64 
70 
72 
75 
76 public:
77 
78  // Provide access to the Matrix& version of evaluateError:
79  using Base::evaluateError;
80 
82  RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
83  const SharedNoiseModel& model) :
84  Base(model, key), i_p_(i_p), c_z_(c_z) {
85  }
86 
88  static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) {
90  // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p
91  iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector());
92  return Rot3(iRc);
93  }
94 
97  return std::static_pointer_cast<gtsam::NonlinearFactor>(
99 
101  void print(const std::string& s = "",
102  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
103  Base::print(s);
104  std::cout << "RotateDirectionsFactor:" << std::endl;
105  i_p_.print("p");
106  c_z_.print("z");
107  }
108 
110  Vector evaluateError(const Rot3& iRc, OptionalMatrixType H) const override {
111  Unit3 i_q = iRc * c_z_;
112  Vector error = i_p_.error(i_q, H);
113  if (H) {
114  Matrix DR;
115  iRc.rotate(c_z_, DR);
116  *H = (*H) * DR;
117  }
118  return error;
119  }
120 
122 };
123 } // namespace gtsam
124 
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::RotateFactor::evaluateError
Vector evaluateError(const Rot3 &R, OptionalMatrixType H) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:56
gtsam::RotateFactor::z_
Point3 z_
Predicted and measured directions, p = iRc * z.
Definition: RotateFactor.h:25
gtsam::RotateFactor::RotateFactor
RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:36
iRc
static const Rot3 iRc(cameraX, cameraY, cameraZ)
gtsam::RotateFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:47
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::RotateDirectionsFactor::Base
NoiseModelFactorN< Rot3 > Base
Definition: RotateFactor.h:73
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
gtsam::RotateFactor
Definition: RotateFactor.h:23
gtsam::NoiseModelFactorN< Rot3 >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::RotateDirectionsFactor::i_p_
Unit3 i_p_
Definition: RotateFactor.h:71
gtsam::Factor
Definition: Factor.h:70
gtsam::Unit3::error
Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q={}) const
Definition: Unit3.cpp:199
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::RotateDirectionsFactor::c_z_
Unit3 c_z_
Predicted and measured directions, i_p = iRc * c_z.
Definition: RotateFactor.h:71
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::RotateFactor::Base
NoiseModelFactorN< Rot3 > Base
Definition: RotateFactor.h:27
Rot3.h
3D rotation represented as a rotation matrix or quaternion
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::KeyFormatter
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
gtsam::RotateDirectionsFactor::RotateDirectionsFactor
RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
Constructor.
Definition: RotateFactor.h:82
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::RotateFactor::p_
Point3 p_
Definition: RotateFactor.h:25
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
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:88
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::RotateDirectionsFactor
Definition: RotateFactor.h:69
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam::RotateFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:42
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam
traits
Definition: SFMdata.h:40
gtsam::RotateDirectionsFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RotateFactor.h:101
P
static double P[]
Definition: ellpe.c:68
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::RotateFactor::This
RotateFactor This
Definition: RotateFactor.h:28
gtsam::NoiseModelFactorN< Rot3 >::key
Key key() const
Definition: NonlinearFactor.h:582
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Z
#define Z
Definition: icosphere.cpp:21
gtsam::RotateDirectionsFactor::evaluateError
Vector evaluateError(const Rot3 &iRc, OptionalMatrixType H) const override
vector of errors returns 2D vector
Definition: RotateFactor.h:110
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
gtsam::Unit3::print
void print(const std::string &s=std::string()) const
The print fuction.
Definition: Unit3.cpp:164
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::RotateDirectionsFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RotateFactor.h:96
gtsam::RotateDirectionsFactor::This
RotateDirectionsFactor This
Definition: RotateFactor.h:74


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:00