Pose3.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
17 #include <gtsam/base/concepts.h>
18 #include <gtsam/geometry/Pose2.h>
19 #include <gtsam/geometry/Pose3.h>
20 #include <gtsam/geometry/Rot3.h>
22 
23 #include <cmath>
24 #include <iostream>
25 #include <string>
26 
27 namespace gtsam {
28 
31 
32 /* ************************************************************************* */
33 Pose3::Pose3(const Pose2& pose2) :
34  R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_(
35  Point3(pose2.x(), pose2.y(), 0)) {
36 }
37 
38 /* ************************************************************************* */
41  if (HR) *HR << I_3x3, Z_3x3;
42  if (Ht) *Ht << Z_3x3, R.transpose();
43  return Pose3(R, t);
44 }
45 
46 // Pose2 constructor Jacobian is always the same.
47 static const Matrix63 Hpose2 = (Matrix63() << //
48  0., 0., 0., //
49  0., 0., 0.,//
50  0., 0., 1.,//
51  1., 0., 0.,//
52  0., 1., 0.,//
53  0., 0., 0.).finished();
54 
56  if (H) *H << Hpose2;
57  return Pose3(p);
58 }
59 
60 /* ************************************************************************* */
62  Rot3 Rt = R_.inverse();
63  return Pose3(Rt, Rt * (-t_));
64 }
65 
66 /* ************************************************************************* */
67 // Calculate Adjoint map
68 // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
70  const Matrix3 R = R_.matrix();
71  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
72  Matrix6 adj;
73  adj << R, Z_3x3, A, R; // Gives [R 0; A R]
74  return adj;
75 }
76 
77 /* ************************************************************************* */
78 // Calculate AdjointMap applied to xi_b, with Jacobians
79 Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
80  OptionalJacobian<6, 6> H_xib) const {
81  const Matrix6 Ad = AdjointMap();
82 
83  // Jacobians
84  // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b
85  // D2 Ad_T(xi_b) = Ad_T
86  // See docs/math.pdf for more details.
87  // In D1 calculation, we could be more efficient by writing it out, but do not
88  // for readability
89  if (H_pose) *H_pose = -Ad * adjointMap(xi_b);
90  if (H_xib) *H_xib = Ad;
91 
92  return Ad * xi_b;
93 }
94 
95 /* ************************************************************************* */
97 Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
98  OptionalJacobian<6, 6> H_x) const {
99  const Matrix6 Ad = AdjointMap();
100  const Vector6 AdTx = Ad.transpose() * x;
101 
102  // Jacobians
103  // See docs/math.pdf for more details.
104  if (H_pose) {
105  const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
106  v_T_hat = skewSymmetric(AdTx.tail<3>());
107  *H_pose << w_T_hat, v_T_hat, //
108  /* */ v_T_hat, Z_3x3;
109  }
110  if (H_x) {
111  *H_x = Ad.transpose();
112  }
113 
114  return AdTx;
115 }
116 
117 /* ************************************************************************* */
118 Matrix6 Pose3::adjointMap(const Vector6& xi) {
119  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
120  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
121  Matrix6 adj;
122  adj << w_hat, Z_3x3, v_hat, w_hat;
123 
124  return adj;
125 }
126 
127 /* ************************************************************************* */
128 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
130  if (Hxi) {
131  Hxi->setZero();
132  for (int i = 0; i < 6; ++i) {
133  Vector6 dxi;
134  dxi.setZero();
135  dxi(i) = 1.0;
136  Matrix6 Gi = adjointMap(dxi);
137  Hxi->col(i) = Gi * y;
138  }
139  }
140  const Matrix6& ad_xi = adjointMap(xi);
141  if (H_y) *H_y = ad_xi;
142  return ad_xi * y;
143 }
144 
145 /* ************************************************************************* */
146 Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
148  if (Hxi) {
149  Hxi->setZero();
150  for (int i = 0; i < 6; ++i) {
151  Vector6 dxi;
152  dxi.setZero();
153  dxi(i) = 1.0;
154  Matrix6 GTi = adjointMap(dxi).transpose();
155  Hxi->col(i) = GTi * y;
156  }
157  }
158  const Matrix6& adT_xi = adjointMap(xi).transpose();
159  if (H_y) *H_y = adT_xi;
160  return adT_xi * y;
161 }
162 
163 /* ************************************************************************* */
164 void Pose3::print(const std::string& s) const {
165  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
166 }
167 
168 /* ************************************************************************* */
169 bool Pose3::equals(const Pose3& pose, double tol) const {
170  return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
171 }
172 
173 /* ************************************************************************* */
174 Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
175  return Pose3(interpolate<Rot3>(R_, T.R_, t),
176  interpolate<Point3>(t_, T.t_, t));
177 }
178 
179 /* ************************************************************************* */
180 // Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's
181 // elegant Lie group document, at https://www.ethaneade.org/lie.pdf.
183  // Get angular velocity omega and translational velocity v from twist xi
184  const Vector3 w = xi.head<3>(), v = xi.tail<3>();
185 
186  // Instantiate functor for Dexp-related operations:
187  const bool nearZero = (w.dot(w) <= 1e-5);
188  const so3::DexpFunctor local(w, nearZero);
189 
190  // Compute rotation using Expmap
191 #ifdef GTSAM_USE_QUATERNIONS
193 #else
194  const Rot3 R(local.expmap());
195 #endif
196 
197  // The translation t = local.leftJacobian() * v.
198  // Here we call applyLeftJacobian, which is faster if you don't need
199  // Jacobians, and returns Jacobian of t with respect to w if asked.
200  // NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
201  // t_parallel = w * w.dot(v); // translation parallel to axis
202  // w_cross_v = w.cross(v); // translation orthogonal to axis
203  // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
204  // but functor does not need R, deals automatically with the case where theta2
205  // is near zero, and also gives us the machinery for the Jacobians.
206  Matrix3 H;
207  const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);
208 
209  if (Hxi) {
210  // The Jacobian of expmap is given by the right Jacobian of SO(3):
211  const Matrix3 Jr = local.rightJacobian();
212  // We are creating a Pose3, so we still need to chain H with R^T, the
213  // Jacobian of Pose3::Create with respect to t.
214  const Matrix3 Q = R.matrix().transpose() * H;
215  *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap
216  Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
217  }
218 
219  return Pose3(R, t);
220 }
221 
222 /* ************************************************************************* */
224  if (Hpose) *Hpose = LogmapDerivative(pose);
225  const Vector3 w = Rot3::Logmap(pose.rotation());
226  const Vector3 T = pose.translation();
227  const double t = w.norm();
228  if (t < 1e-10) {
229  Vector6 log;
230  log << w, T;
231  return log;
232  } else {
233  const Matrix3 W = skewSymmetric(w / t);
234  // Formula from Agrawal06iros, equation (14)
235  // simplified with Mathematica, and multiplying in T to avoid matrix math
236  const double Tan = tan(0.5 * t);
237  const Vector3 WT = W * T;
238  const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
239  Vector6 log;
240  log << w, u;
241  return log;
242  }
243 }
244 
245 /* ************************************************************************* */
247 #ifdef GTSAM_POSE3_EXPMAP
248  return Expmap(xi, Hxi);
249 #else
250  Matrix3 DR;
251  Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0);
252  if (Hxi) {
253  *Hxi = I_6x6;
254  Hxi->topLeftCorner<3, 3>() = DR;
255  }
256  return Pose3(R, Point3(xi.tail<3>()));
257 #endif
258 }
259 
260 /* ************************************************************************* */
262 #ifdef GTSAM_POSE3_EXPMAP
263  return Logmap(pose, Hpose);
264 #else
265  Matrix3 DR;
266  Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0);
267  if (Hpose) {
268  *Hpose = I_6x6;
269  Hpose->topLeftCorner<3, 3>() = DR;
270  }
271  Vector6 xi;
272  xi << omega, pose.translation();
273  return xi;
274 #endif
275 }
276 
277 /* ************************************************************************* */
278 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
279  double nearZeroThreshold) {
280  const auto w = xi.head<3>();
281  const auto v = xi.tail<3>();
282 
283  // Instantiate functor for Dexp-related operations:
284  bool nearZero = (w.dot(w) <= nearZeroThreshold);
285  so3::DexpFunctor local(w, nearZero);
286 
287  // Call applyLeftJacobian to get its Jacobian
288  Matrix3 H;
289  local.applyLeftJacobian(v, H);
290 
291  // Multiply with R^T to account for the Pose3::Create Jacobian.
292  const Matrix3 R = local.expmap();
293  return R.transpose() * H;
294 }
295 
296 /* ************************************************************************* */
298  Matrix6 J;
299  Expmap(xi, J);
300  return J;
301 }
302 
303 /* ************************************************************************* */
305  const Vector6 xi = Logmap(pose);
306  const Vector3 w = xi.head<3>();
307  const Matrix3 Jw = Rot3::LogmapDerivative(w);
308  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
309  const Matrix3 Q2 = -Jw*Q*Jw;
310  Matrix6 J;
311  J << Jw, Z_3x3, Q2, Jw;
312  return J;
313 }
314 
315 /* ************************************************************************* */
317  if (Hself) *Hself << Z_3x3, rotation().matrix();
318  return t_;
319 }
320 
321 /* ************************************************************************* */
323  if (Hself) {
324  *Hself << I_3x3, Z_3x3;
325  }
326  return R_;
327 }
328 
329 /* ************************************************************************* */
330 Matrix4 Pose3::matrix() const {
331  static const auto A14 = Eigen::RowVector4d(0,0,0,1);
332  Matrix4 mat;
333  mat << R_.matrix(), t_, A14;
334  return mat;
335 }
336 
337 /* ************************************************************************* */
339  OptionalJacobian<6, 6> HaTb) const {
340  const Pose3& wTa = *this;
341  return wTa.compose(aTb, Hself, HaTb);
342 }
343 
344 /* ************************************************************************* */
346  OptionalJacobian<6, 6> HwTb) const {
347  if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
348  if (HwTb) *HwTb = I_6x6;
349  const Pose3& wTa = *this;
350  return wTa.inverse() * wTb;
351 }
352 
353 /* ************************************************************************* */
355  OptionalJacobian<3, 3> Hpoint) const {
356  // Only get matrix once, to avoid multiple allocations,
357  // as well as multiple conversions in the Quaternion case
358  const Matrix3 R = R_.matrix();
359  if (Hself) {
360  Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z());
361  Hself->rightCols<3>() = R;
362  }
363  if (Hpoint) {
364  *Hpoint = R;
365  }
366  return R_ * point + t_;
367 }
368 
369 Matrix Pose3::transformFrom(const Matrix& points) const {
370  if (points.rows() != 3) {
371  throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
372  }
373  const Matrix3 R = R_.matrix();
374  return (R * points).colwise() + t_; // Eigen broadcasting!
375 }
376 
377 /* ************************************************************************* */
379  OptionalJacobian<3, 3> Hpoint) const {
380  // Only get transpose once, to avoid multiple allocations,
381  // as well as multiple conversions in the Quaternion case
382  const Matrix3 Rt = R_.transpose();
383  const Point3 q(Rt*(point - t_));
384  if (Hself) {
385  const double wx = q.x(), wy = q.y(), wz = q.z();
386  (*Hself) <<
387  0.0, -wz, +wy,-1.0, 0.0, 0.0,
388  +wz, 0.0, -wx, 0.0,-1.0, 0.0,
389  -wy, +wx, 0.0, 0.0, 0.0,-1.0;
390  }
391  if (Hpoint) {
392  *Hpoint = Rt;
393  }
394  return q;
395 }
396 
397 Matrix Pose3::transformTo(const Matrix& points) const {
398  if (points.rows() != 3) {
399  throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
400  }
401  const Matrix3 Rt = R_.transpose();
402  return Rt * (points.colwise() - t_); // Eigen broadcasting!
403 }
404 
405 /* ************************************************************************* */
407  OptionalJacobian<1, 3> Hpoint) const {
408  Matrix36 D_local_pose;
409  Matrix3 D_local_point;
410  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
411  if (!Hself && !Hpoint) {
412  return local.norm();
413  } else {
414  Matrix13 D_r_local;
415  const double r = norm3(local, D_r_local);
416  if (Hself) *Hself = D_r_local * D_local_pose;
417  if (Hpoint) *Hpoint = D_r_local * D_local_point;
418  return r;
419  }
420 }
421 
422 /* ************************************************************************* */
424  OptionalJacobian<1, 6> Hpose) const {
425  Matrix13 D_local_point;
426  double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
427  if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
428  return r;
429 }
430 
431 /* ************************************************************************* */
433  OptionalJacobian<2, 3> Hpoint) const {
434  Matrix36 D_local_pose;
435  Matrix3 D_local_point;
436  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
437  if (!Hself && !Hpoint) {
438  return Unit3(local);
439  } else {
440  Matrix23 D_b_local;
441  Unit3 b = Unit3::FromPoint3(local, D_b_local);
442  if (Hself) *Hself = D_b_local * D_local_pose;
443  if (Hpoint) *Hpoint = D_b_local * D_local_point;
444  return b;
445  }
446 }
447 
448 /* ************************************************************************* */
450  OptionalJacobian<2, 6> Hpose) const {
451  if (Hpose) {
452  Hpose->setZero();
453  return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
454  }
455  return bearing(pose.translation(), Hself, {});
456 }
457 
458 /* ************************************************************************* */
459 std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
460  const size_t n = abPointPairs.size();
461  if (n < 3) {
462  return {}; // we need at least three pairs
463  }
464 
465  // calculate centroids
466  const auto centroids = means(abPointPairs);
467 
468  // Add to form H matrix
469  Matrix3 H = Z_3x3;
470  for (const Point3Pair &abPair : abPointPairs) {
471  const Point3 da = abPair.first - centroids.first;
472  const Point3 db = abPair.second - centroids.second;
473  H += da * db.transpose();
474  }
475 
476  // ClosestTo finds rotation matrix closest to H in Frobenius sense
477  const Rot3 aRb = Rot3::ClosestTo(H);
478  const Point3 aTb = centroids.first - aRb * centroids.second;
479  return Pose3(aRb, aTb);
480 }
481 
482 std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
483  if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
484  throw std::invalid_argument(
485  "Pose3:Align expects 3*N matrices of equal shape.");
486  }
487  Point3Pairs abPointPairs;
488  for (Eigen::Index j = 0; j < a.cols(); j++) {
489  abPointPairs.emplace_back(a.col(j), b.col(j));
490  }
491  return Pose3::Align(abPointPairs);
492 }
493 
494 /* ************************************************************************* */
496  return interpolate(*this, other, t, Hx, Hy);
497 }
498 
499 /* ************************************************************************* */
500 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
501  // Both Rot3 and Point3 have ostream definitions so we use them.
502  os << "R: " << pose.rotation() << "\n";
503  os << "t: " << pose.translation().transpose();
504  return os;
505 }
506 
507 } // namespace gtsam
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::Pose3::slerp
Pose3 slerp(double t, const Pose3 &other, OptionalJacobian< 6, 6 > Hx={}, OptionalJacobian< 6, 6 > Hy={}) const
Spherical Linear interpolation between *this and other.
Definition: Pose3.cpp:495
gtsam::Pose3::interpolateRt
Pose3 interpolateRt(const Pose3 &T, double t) const
Definition: Pose3.cpp:174
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::Point3Pair
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42
Pose2.h
2D Pose
gtsam::Pose3::R_
Rot3 R_
Rotation gRp, between global and pose frame.
Definition: Pose3.h:46
gtsam::Pose3::ComputeQforExpmapDerivative
static Matrix3 ComputeQforExpmapDerivative(const Vector6 &xi, double nearZeroThreshold=1e-5)
Definition: Pose3.cpp:278
gtsam::Point3Pairs
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:378
aRb
Rot3 aRb
Definition: testFundamentalMatrix.cpp:146
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Pose3::FromPose2
static Pose3 FromPose2(const Pose2 &p, OptionalJacobian< 6, 3 > H={})
Definition: Pose3.cpp:55
gtsam::Pose3::adjointTranspose
static Vector6 adjointTranspose(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
Definition: Pose3.cpp:146
gtsam::Rot3::ClosestTo
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:275
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::Pose3::Logmap
static Vector6 Logmap(const Pose3 &pose, OptionalJacobian< 6, 6 > Hpose={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3.cpp:223
x
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 x
Definition: gnuplot_common_settings.hh:12
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:182
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
concepts.h
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
mat
MatrixXf mat
Definition: Tutorial_AdvancedInitialization_CommaTemporary.cpp:1
os
ofstream os("timeSchurFactors.csv")
gtsam::Pose3::t_
Point3 t_
Translation gPp, from global origin to pose frame origin.
Definition: Pose3.h:47
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:394
Q
Quaternion Q
Definition: testQuaternion.cpp:27
gtsam::Rot3::LogmapDerivative
static Matrix3 LogmapDerivative(const Vector3 &x)
Derivative of Logmap.
Definition: Rot3.cpp:242
gtsam::Pose3::Pose3
Pose3()
Definition: Pose3.h:55
gtsam::Pose3::matrix
Matrix4 matrix() const
Definition: Pose3.cpp:330
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Definition: Pose3.cpp:406
Rot3.h
3D rotation represented as a rotation matrix or quaternion
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
gtsam::Pose3::transformPoseFrom
Pose3 transformPoseFrom(const Pose3 &aTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HaTb={}) const
Definition: Pose3.cpp:338
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::Pose3::AdjointTranspose
Vector6 AdjointTranspose(const Vector6 &x, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_x={}) const
The dual version of Adjoint.
Definition: Pose3.cpp:97
gtsam::Pose3::print
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:164
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::Pose3::transformPoseTo
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
Definition: Pose3.cpp:345
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Pose3::Adjoint
Vector6 Adjoint(const Vector6 &xi_b, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_xib={}) const
Definition: Pose3.cpp:79
gtsam::LieGroup< Rot3, 3 >::Retract
static Rot3 Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:111
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
A
Definition: test_numpy_dtypes.cpp:298
gtsam::Pose3::Create
static Pose3 Create(const Rot3 &R, const Point3 &t, OptionalJacobian< 6, 3 > HR={}, OptionalJacobian< 6, 3 > Ht={})
Named constructor with derivatives.
Definition: Pose3.cpp:39
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
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::Pose3
Definition: Pose3.h:37
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:61
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Definition: Lie.h:327
gtsam::Pose3::ExpmapDerivative
static Matrix6 ExpmapDerivative(const Vector6 &xi)
Derivative of Expmap.
Definition: Pose3.cpp:297
gtsam::Pose3::equals
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:169
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:316
gtsam::Pose3::AdjointMap
Matrix6 AdjointMap() const
Definition: Pose3.cpp:69
gtsam::Pose3::y
double y() const
get y
Definition: Pose3.h:300
example2::aTb
Point3 aTb
Definition: testEssentialMatrixFactor.cpp:585
gtsam::Pose3::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose3 &p)
Output stream operator.
Definition: Pose3.cpp:500
Eigen::Triplet< double >
tan
const EIGEN_DEVICE_FUNC TanReturnType tan() const
Definition: ArrayCwiseUnaryOps.h:269
gtsam::Pose3::ChartAtOrigin::Retract
static Pose3 Retract(const Vector6 &xi, ChartJacobian Hxi={})
Definition: Pose3.cpp:246
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:312
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::so3::DexpFunctor
Definition: SO3.h:162
Q2
static double Q2[8]
Definition: ndtri.c:122
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Pose3::bearing
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:432
gtsam::Pose3::ChartAtOrigin::Local
static Vector6 Local(const Pose3 &pose, ChartJacobian Hpose={})
Definition: Pose3.cpp:261
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:322
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Definition: OptionalJacobian.h:172
gtsam::Pose3::adjoint
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
Definition: Pose3.cpp:128
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:41
GTSAM_CONCEPT_POSE_INST
#define GTSAM_CONCEPT_POSE_INST(T)
Definition: geometry/concepts.h:73
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::symbol_shorthand::W
Key W(std::uint64_t j)
Definition: inference/Symbol.h:170
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:354
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:295
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Rot3::Logmap
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
Eigen::Matrix< double, 6, 6 >
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Pose3::LogmapDerivative
static Matrix6 LogmapDerivative(const Pose3 &xi)
Derivative of Logmap.
Definition: Pose3.cpp:304
gtsam::LieGroup< Rot3, 3 >::LocalCoordinates
static TangentVector LocalCoordinates(const Rot3 &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:116
gtsam::Pose3::adjointMap
static Matrix6 adjointMap(const Vector6 &xi)
Definition: Pose3.cpp:118
align_3::t
Point2 t(10, 10)
gtsam::Hpose2
static const Matrix63 Hpose2
Definition: Pose3.cpp:47
gtsam::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:100
Pose3
Definition: testDependencies.h:3
gtsam::Pose3::Align
static std::optional< Pose3 > Align(const Point3Pairs &abPointPairs)
Definition: Pose3.cpp:459
gtsam::Unit3::FromPoint3
static Unit3 FromPoint3(const Point3 &point, OptionalJacobian< 2, 3 > H={})
Named constructor from Point3 with optional Jacobian.
Definition: Unit3.cpp:44
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::Pose2
Definition: Pose2.h:39
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
HR
#define HR
Definition: mincover.c:26


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:02:36