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/geometry/Pose3.h>
18 #include <gtsam/geometry/Pose2.h>
20 #include <gtsam/base/concepts.h>
21 
22 #include <cmath>
23 #include <iostream>
24 #include <limits>
25 #include <string>
26 
27 namespace gtsam {
28 
29 using std::vector;
30 
33 
34 /* ************************************************************************* */
35 Pose3::Pose3(const Pose2& pose2) :
36  R_(Rot3::Rodrigues(0, 0, pose2.theta())), t_(
37  Point3(pose2.x(), pose2.y(), 0)) {
38 }
39 
40 /* ************************************************************************* */
43  if (HR) *HR << I_3x3, Z_3x3;
44  if (Ht) *Ht << Z_3x3, R.transpose();
45  return Pose3(R, t);
46 }
47 
48 /* ************************************************************************* */
50  Rot3 Rt = R_.inverse();
51  return Pose3(Rt, Rt * (-t_));
52 }
53 
54 /* ************************************************************************* */
55 // Calculate Adjoint map
56 // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
57 Matrix6 Pose3::AdjointMap() const {
58  const Matrix3 R = R_.matrix();
59  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
60  Matrix6 adj;
61  adj << R, Z_3x3, A, R; // Gives [R 0; A R]
62  return adj;
63 }
64 
65 /* ************************************************************************* */
66 // Calculate AdjointMap applied to xi_b, with Jacobians
67 Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
68  OptionalJacobian<6, 6> H_xib) const {
69  const Matrix6 Ad = AdjointMap();
70 
71  // Jacobians
72  // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b
73  // D2 Ad_T(xi_b) = Ad_T
74  // See docs/math.pdf for more details.
75  // In D1 calculation, we could be more efficient by writing it out, but do not
76  // for readability
77  if (H_pose) *H_pose = -Ad * adjointMap(xi_b);
78  if (H_xib) *H_xib = Ad;
79 
80  return Ad * xi_b;
81 }
82 
83 /* ************************************************************************* */
85 Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
86  OptionalJacobian<6, 6> H_x) const {
87  const Matrix6 Ad = AdjointMap();
88  const Vector6 AdTx = Ad.transpose() * x;
89 
90  // Jacobians
91  // See docs/math.pdf for more details.
92  if (H_pose) {
93  const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
94  v_T_hat = skewSymmetric(AdTx.tail<3>());
95  *H_pose << w_T_hat, v_T_hat, //
96  /* */ v_T_hat, Z_3x3;
97  }
98  if (H_x) {
99  *H_x = Ad.transpose();
100  }
101 
102  return AdTx;
103 }
104 
105 /* ************************************************************************* */
106 Matrix6 Pose3::adjointMap(const Vector6& xi) {
107  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
108  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
109  Matrix6 adj;
110  adj << w_hat, Z_3x3, v_hat, w_hat;
111 
112  return adj;
113 }
114 
115 /* ************************************************************************* */
116 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
118  if (Hxi) {
119  Hxi->setZero();
120  for (int i = 0; i < 6; ++i) {
121  Vector6 dxi;
122  dxi.setZero();
123  dxi(i) = 1.0;
124  Matrix6 Gi = adjointMap(dxi);
125  Hxi->col(i) = Gi * y;
126  }
127  }
128  const Matrix6& ad_xi = adjointMap(xi);
129  if (H_y) *H_y = ad_xi;
130  return ad_xi * y;
131 }
132 
133 /* ************************************************************************* */
134 Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
136  if (Hxi) {
137  Hxi->setZero();
138  for (int i = 0; i < 6; ++i) {
139  Vector6 dxi;
140  dxi.setZero();
141  dxi(i) = 1.0;
142  Matrix6 GTi = adjointMap(dxi).transpose();
143  Hxi->col(i) = GTi * y;
144  }
145  }
146  const Matrix6& adT_xi = adjointMap(xi).transpose();
147  if (H_y) *H_y = adT_xi;
148  return adT_xi * y;
149 }
150 
151 /* ************************************************************************* */
152 void Pose3::print(const std::string& s) const {
153  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
154 }
155 
156 /* ************************************************************************* */
157 bool Pose3::equals(const Pose3& pose, double tol) const {
158  return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
159 }
160 
161 /* ************************************************************************* */
162 Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
163  return Pose3(interpolate<Rot3>(R_, T.R_, t),
164  interpolate<Point3>(t_, T.t_, t));
165 }
166 
167 /* ************************************************************************* */
170  if (Hxi) *Hxi = ExpmapDerivative(xi);
171 
172  // get angular velocity omega and translational velocity v from twist xi
173  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
174 
176  double theta2 = omega.dot(omega);
177  if (theta2 > std::numeric_limits<double>::epsilon()) {
178  Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
179  Vector3 omega_cross_v = omega.cross(v); // points towards axis
180  Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
181  return Pose3(R, t);
182  } else {
183  return Pose3(R, v);
184  }
185 }
186 
187 /* ************************************************************************* */
189  if (Hpose) *Hpose = LogmapDerivative(pose);
190  const Vector3 w = Rot3::Logmap(pose.rotation());
191  const Vector3 T = pose.translation();
192  const double t = w.norm();
193  if (t < 1e-10) {
194  Vector6 log;
195  log << w, T;
196  return log;
197  } else {
198  const Matrix3 W = skewSymmetric(w / t);
199  // Formula from Agrawal06iros, equation (14)
200  // simplified with Mathematica, and multiplying in T to avoid matrix math
201  const double Tan = tan(0.5 * t);
202  const Vector3 WT = W * T;
203  const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
204  Vector6 log;
205  log << w, u;
206  return log;
207  }
208 }
209 
210 /* ************************************************************************* */
212 #ifdef GTSAM_POSE3_EXPMAP
213  return Expmap(xi, Hxi);
214 #else
215  Matrix3 DR;
216  Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0);
217  if (Hxi) {
218  *Hxi = I_6x6;
219  Hxi->topLeftCorner<3, 3>() = DR;
220  }
221  return Pose3(R, Point3(xi.tail<3>()));
222 #endif
223 }
224 
225 /* ************************************************************************* */
227 #ifdef GTSAM_POSE3_EXPMAP
228  return Logmap(pose, Hpose);
229 #else
230  Matrix3 DR;
231  Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0);
232  if (Hpose) {
233  *Hpose = I_6x6;
234  Hpose->topLeftCorner<3, 3>() = DR;
235  }
236  Vector6 xi;
237  xi << omega, pose.translation();
238  return xi;
239 #endif
240 }
241 
242 /* ************************************************************************* */
243 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
244  const auto w = xi.head<3>();
245  const auto v = xi.tail<3>();
246  const Matrix3 V = skewSymmetric(v);
247  const Matrix3 W = skewSymmetric(w);
248 
249  Matrix3 Q;
250 
251 #ifdef NUMERICAL_EXPMAP_DERIV
252  Matrix3 Qj = Z_3x3;
253  double invFac = 1.0;
254  Q = Z_3x3;
255  Matrix3 Wj = I_3x3;
256  for (size_t j=1; j<10; ++j) {
257  Qj = Qj*W + Wj*V;
258  invFac = -invFac/(j+1);
259  Q = Q + invFac*Qj;
260  Wj = Wj*W;
261  }
262 #else
263  // The closed-form formula in Barfoot14tro eq. (102)
264  double phi = w.norm();
265  const Matrix3 WVW = W * V * W;
266  if (std::abs(phi) > nearZeroThreshold) {
267  const double s = sin(phi), c = cos(phi);
268  const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
269  phi5 = phi4 * phi;
270  // Invert the sign of odd-order terms to have the right Jacobian
271  Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
272  (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
273  0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
274  (WVW * W + W * WVW);
275  } else {
276  Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
277  1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
278  1. / 120. * (WVW * W + W * WVW);
279  }
280 #endif
281 
282  return Q;
283 }
284 
285 /* ************************************************************************* */
286 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
287  const Vector3 w = xi.head<3>();
288  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
289  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
290  Matrix6 J;
291  J << Jw, Z_3x3, Q, Jw;
292  return J;
293 }
294 
295 /* ************************************************************************* */
297  const Vector6 xi = Logmap(pose);
298  const Vector3 w = xi.head<3>();
299  const Matrix3 Jw = Rot3::LogmapDerivative(w);
300  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
301  const Matrix3 Q2 = -Jw*Q*Jw;
302  Matrix6 J;
303  J << Jw, Z_3x3, Q2, Jw;
304  return J;
305 }
306 
307 /* ************************************************************************* */
309  if (Hself) *Hself << Z_3x3, rotation().matrix();
310  return t_;
311 }
312 
313 /* ************************************************************************* */
314 
316  if (Hself) {
317  *Hself << I_3x3, Z_3x3;
318  }
319  return R_;
320 }
321 
322 /* ************************************************************************* */
323 Matrix4 Pose3::matrix() const {
324  static const auto A14 = Eigen::RowVector4d(0,0,0,1);
325  Matrix4 mat;
326  mat << R_.matrix(), t_, A14;
327  return mat;
328 }
329 
330 /* ************************************************************************* */
332  OptionalJacobian<6, 6> HaTb) const {
333  const Pose3& wTa = *this;
334  return wTa.compose(aTb, Hself, HaTb);
335 }
336 
337 /* ************************************************************************* */
339  OptionalJacobian<6, 6> HwTb) const {
340  if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
341  if (HwTb) *HwTb = I_6x6;
342  const Pose3& wTa = *this;
343  return wTa.inverse() * wTb;
344 }
345 
346 /* ************************************************************************* */
348  OptionalJacobian<3, 3> Hpoint) const {
349  // Only get matrix once, to avoid multiple allocations,
350  // as well as multiple conversions in the Quaternion case
351  const Matrix3 R = R_.matrix();
352  if (Hself) {
353  Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z());
354  Hself->rightCols<3>() = R;
355  }
356  if (Hpoint) {
357  *Hpoint = R;
358  }
359  return R_ * point + t_;
360 }
361 
362 Matrix Pose3::transformFrom(const Matrix& points) const {
363  if (points.rows() != 3) {
364  throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
365  }
366  const Matrix3 R = R_.matrix();
367  return (R * points).colwise() + t_; // Eigen broadcasting!
368 }
369 
370 /* ************************************************************************* */
372  OptionalJacobian<3, 3> Hpoint) const {
373  // Only get transpose once, to avoid multiple allocations,
374  // as well as multiple conversions in the Quaternion case
375  const Matrix3 Rt = R_.transpose();
376  const Point3 q(Rt*(point - t_));
377  if (Hself) {
378  const double wx = q.x(), wy = q.y(), wz = q.z();
379  (*Hself) <<
380  0.0, -wz, +wy,-1.0, 0.0, 0.0,
381  +wz, 0.0, -wx, 0.0,-1.0, 0.0,
382  -wy, +wx, 0.0, 0.0, 0.0,-1.0;
383  }
384  if (Hpoint) {
385  *Hpoint = Rt;
386  }
387  return q;
388 }
389 
390 Matrix Pose3::transformTo(const Matrix& points) const {
391  if (points.rows() != 3) {
392  throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
393  }
394  const Matrix3 Rt = R_.transpose();
395  return Rt * (points.colwise() - t_); // Eigen broadcasting!
396 }
397 
398 /* ************************************************************************* */
400  OptionalJacobian<1, 3> Hpoint) const {
401  Matrix36 D_local_pose;
402  Matrix3 D_local_point;
403  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
404  if (!Hself && !Hpoint) {
405  return local.norm();
406  } else {
407  Matrix13 D_r_local;
408  const double r = norm3(local, D_r_local);
409  if (Hself) *Hself = D_r_local * D_local_pose;
410  if (Hpoint) *Hpoint = D_r_local * D_local_point;
411  return r;
412  }
413 }
414 
415 /* ************************************************************************* */
417  OptionalJacobian<1, 6> Hpose) const {
418  Matrix13 D_local_point;
419  double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
420  if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
421  return r;
422 }
423 
424 /* ************************************************************************* */
426  OptionalJacobian<2, 3> Hpoint) const {
427  Matrix36 D_local_pose;
428  Matrix3 D_local_point;
429  Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
430  if (!Hself && !Hpoint) {
431  return Unit3(local);
432  } else {
433  Matrix23 D_b_local;
434  Unit3 b = Unit3::FromPoint3(local, D_b_local);
435  if (Hself) *Hself = D_b_local * D_local_pose;
436  if (Hpoint) *Hpoint = D_b_local * D_local_point;
437  return b;
438  }
439 }
440 
441 /* ************************************************************************* */
443  OptionalJacobian<2, 6> Hpose) const {
444  if (Hpose) {
445  Hpose->setZero();
446  return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
447  }
448  return bearing(pose.translation(), Hself, {});
449 }
450 
451 /* ************************************************************************* */
452 std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
453  const size_t n = abPointPairs.size();
454  if (n < 3) {
455  return {}; // we need at least three pairs
456  }
457 
458  // calculate centroids
459  const auto centroids = means(abPointPairs);
460 
461  // Add to form H matrix
462  Matrix3 H = Z_3x3;
463  for (const Point3Pair &abPair : abPointPairs) {
464  const Point3 da = abPair.first - centroids.first;
465  const Point3 db = abPair.second - centroids.second;
466  H += da * db.transpose();
467  }
468 
469  // ClosestTo finds rotation matrix closest to H in Frobenius sense
470  const Rot3 aRb = Rot3::ClosestTo(H);
471  const Point3 aTb = centroids.first - aRb * centroids.second;
472  return Pose3(aRb, aTb);
473 }
474 
475 std::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
476  if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
477  throw std::invalid_argument(
478  "Pose3:Align expects 3*N matrices of equal shape.");
479  }
480  Point3Pairs abPointPairs;
481  for (Eigen::Index j = 0; j < a.cols(); j++) {
482  abPointPairs.emplace_back(a.col(j), b.col(j));
483  }
484  return Pose3::Align(abPointPairs);
485 }
486 
487 /* ************************************************************************* */
489  return interpolate(*this, other, t, Hx, Hy);
490 }
491 
492 /* ************************************************************************* */
493 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
494  // Both Rot3 and Point3 have ostream definitions so we use them.
495  os << "R: " << pose.rotation() << "\n";
496  os << "t: " << pose.translation().transpose();
497  return os;
498 }
499 
500 } // namespace gtsam
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi={})
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose3.cpp:169
#define GTSAM_CONCEPT_POSE_INST(T)
Point3 t_
Translation gPp, from global origin to pose frame origin.
Definition: Pose3.h:47
static Matrix6 adjointMap(const Vector6 &xi)
Definition: Pose3.cpp:106
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Quaternion Q
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:99
static Vector6 Logmap(const Pose3 &pose, OptionalJacobian< 6, 6 > Hpose={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3.cpp:188
Rot3 R_
Rotation gRp, between global and pose frame.
Definition: Pose3.h:46
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
static Matrix6 LogmapDerivative(const Pose3 &xi)
Derivative of Logmap.
Definition: Pose3.cpp:296
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose3 transformPoseFrom(const Pose3 &aTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HaTb={}) const
Definition: Pose3.cpp:331
Key W(std::uint64_t j)
OptionalJacobian< Rows, N > cols(int startCol)
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
static Matrix6 ExpmapDerivative(const Vector6 &xi)
Derivative of Expmap.
Definition: Pose3.cpp:286
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:157
static Vector6 Local(const Pose3 &pose, ChartJacobian Hpose={})
Definition: Pose3.cpp:226
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:488
EIGEN_DEVICE_FUNC const LogReturnType log() const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Pose3 Retract(const Vector6 &xi, ChartJacobian Hxi={})
Definition: Pose3.cpp:211
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
static double epsilon
Definition: testRot3.cpp:37
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
static Matrix3 ComputeQforExpmapDerivative(const Vector6 &xi, double nearZeroThreshold=1e-5)
Definition: Pose3.cpp:243
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose3 &p)
Output stream operator.
Definition: Pose3.cpp:493
static Matrix3 LogmapDerivative(const Vector3 &x)
Derivative of Logmap.
Definition: Rot3.cpp:241
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition: Rot3.cpp:236
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:425
Vector6 AdjointTranspose(const Vector6 &x, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_x={}) const
The dual version of Adjoint.
Definition: Pose3.cpp:85
Array< int, Dynamic, 1 > v
Eigen::Triplet< double > T
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:347
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
JacobiRotation< float > J
Class compose(const Class &g) const
Definition: Lie.h:48
const G & b
Definition: Group.h:86
RowVector3d w
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
static Unit3 FromPoint3(const Point3 &point, OptionalJacobian< 2, 3 > H={})
Named constructor from Point3 with optional Jacobian.
Definition: Unit3.cpp:44
static std::optional< Pose3 > Align(const Point3Pairs &abPointPairs)
Definition: Pose3.cpp:452
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
double x() const
get x
Definition: Pose3.h:292
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:152
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
ofstream os("timeSchurFactors.csv")
static TangentVector LocalCoordinates(const Rot3 &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition: Lie.h:116
The quaternion class used to represent 3D orientations and rotations.
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:371
Matrix3 matrix() const
Definition: Rot3M.cpp:218
EIGEN_DEVICE_FUNC const TanReturnType tan() const
static Vector6 adjointTranspose(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
Definition: Pose3.cpp:134
#define HR
Definition: mincover.c:26
static Pose3 pose2
static Rot3 Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:111
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:271
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Pose3 interpolateRt(const Pose3 &T, double t) const
Definition: Pose3.cpp:162
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:41
Matrix6 AdjointMap() const
Definition: Pose3.cpp:57
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
2D Pose
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
Definition: Pose3.cpp:338
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
static Pose3 Create(const Rot3 &R, const Point3 &t, OptionalJacobian< 6, 3 > HR={}, OptionalJacobian< 6, 3 > Ht={})
Named constructor with derivatives.
Definition: Pose3.cpp:41
#define abs(x)
Definition: datatypes.h:17
3D Pose
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
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
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian< 6, 6 > Hxi={}, OptionalJacobian< 6, 6 > H_y={})
Definition: Pose3.cpp:116
Matrix3 transpose() const
Definition: Rot3M.cpp:143
Vector6 Adjoint(const Vector6 &xi_b, OptionalJacobian< 6, 6 > H_this={}, OptionalJacobian< 6, 6 > H_xib={}) const
Definition: Pose3.cpp:67
Matrix4 matrix() const
Definition: Pose3.cpp:323
std::ptrdiff_t j
Point2 t(10, 10)
double y() const
get y
Definition: Pose3.h:297
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Definition: Pose3.cpp:399
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:14