Pose2.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 
18 #include <gtsam/geometry/Pose2.h>
19 #include <gtsam/base/Testable.h>
20 #include <gtsam/base/concepts.h>
21 
22 #include <cmath>
23 #include <iostream>
24 #include <iomanip>
25 
26 using namespace std;
27 
28 namespace gtsam {
29 
32 
33 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
34 
35 /* ************************************************************************* */
36 Matrix3 Pose2::matrix() const {
37  Matrix2 R = r_.matrix();
38  Matrix32 R0;
39  R0.block<2,2>(0,0) = R;
40  R0.block<1,2>(2,0).setZero();
41  Matrix31 T;
42  T << t_.x(), t_.y(), 1.0;
43  Matrix3 RT_;
44  RT_.block<3,2>(0,0) = R0;
45  RT_.block<3,1>(0,2) = T;
46  return RT_;
47 }
48 
49 /* ************************************************************************* */
50 void Pose2::print(const string& s) const {
51  std::cout << (s.empty() ? s : s + " ") << *this << std::endl;
52 }
53 
54 /* ************************************************************************* */
55 std::ostream &operator<<(std::ostream &os, const Pose2& pose) {
56  os << "(" << pose.x() << ", " << pose.y() << ", " << pose.theta() << ")";
57  return os;
58 }
59 
60 /* ************************************************************************* */
61 bool Pose2::equals(const Pose2& q, double tol) const {
62  return equal_with_abs_tol(t_, q.t_, tol) && r_.equals(q.r_, tol);
63 }
64 
65 /* ************************************************************************* */
67  assert(xi.size() == 3);
68  if (H) *H = Pose2::ExpmapDerivative(xi);
69  const Point2 v(xi(0),xi(1));
70  const double w = xi(2);
71  if (std::abs(w) < 1e-10)
72  return Pose2(xi[0], xi[1], xi[2]);
73  else {
74  const Rot2 R(Rot2::fromAngle(w));
75  const Point2 v_ortho = R_PI_2 * v; // points towards rot center
76  const Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
77  return Pose2(R, t);
78  }
79 }
80 
81 /* ************************************************************************* */
82 Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
83  if (H) *H = Pose2::LogmapDerivative(p);
84  const Rot2& R = p.r();
85  const Point2& t = p.t();
86  double w = R.theta();
87  if (std::abs(w) < 1e-10)
88  return Vector3(t.x(), t.y(), w);
89  else {
90  double c_1 = R.c()-1.0, s = R.s();
91  double det = c_1*c_1 + s*s;
92  Point2 p = R_PI_2 * (R.unrotate(t) - t);
93  Point2 v = (w/det) * p;
94  return Vector3(v.x(), v.y(), w);
95  }
96 }
97 
98 /* ************************************************************************* */
99 Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
100 #ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
101  return Expmap(v, H);
102 #else
103  if (H) {
104  *H = I_3x3;
105  H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix();
106  }
107  return Pose2(v[0], v[1], v[2]);
108 #endif
109 }
110 /* ************************************************************************* */
111 Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
112 #ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP
113  return Logmap(r, H);
114 #else
115  if (H) {
116  *H = I_3x3;
117  H->topLeftCorner<2,2>() = r.rotation().matrix();
118  }
119  return Vector3(r.x(), r.y(), r.theta());
120 #endif
121 }
122 
123 /* ************************************************************************* */
124 // Calculate Adjoint map
125 // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
126 Matrix3 Pose2::AdjointMap() const {
127  double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
128  Matrix3 rvalue;
129  rvalue <<
130  c, -s, y,
131  s, c, -x,
132  0.0, 0.0, 1.0;
133  return rvalue;
134 }
135 
136 /* ************************************************************************* */
137 Matrix3 Pose2::adjointMap(const Vector3& v) {
138  // See Chirikjian12book2, vol.2, pg. 36
139  Matrix3 ad = Z_3x3;
140  ad(0,1) = -v[2];
141  ad(1,0) = v[2];
142  ad(0,2) = v[1];
143  ad(1,2) = -v[0];
144  return ad;
145 }
146 
147 /* ************************************************************************* */
148 Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
149  double alpha = v[2];
150  Matrix3 J;
151  if (std::abs(alpha) > 1e-5) {
152  // Chirikjian11book2, pg. 36
153  /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26
154  * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation
155  * In fact, Iserles 2.42 can be written as:
156  * \dot{g} g^{-1} = dexpR_{q}\dot{q}
157  * where q = A, and g = exp(A)
158  * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26.
159  * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36
160  */
161  double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
162  double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
163  J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
164  c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
165  0, 0, 1;
166  }
167  else {
168  // Thanks to Krunal: Apply L'Hospital rule to several times to
169  // compute the limits when alpha -> 0
170  J << 1,0,-0.5*v[1],
171  0,1, 0.5*v[0],
172  0,0, 1;
173  }
174 
175  return J;
176 }
177 
178 /* ************************************************************************* */
179 Matrix3 Pose2::LogmapDerivative(const Pose2& p) {
180  Vector3 v = Logmap(p);
181  double alpha = v[2];
182  Matrix3 J;
183  if (std::abs(alpha) > 1e-5) {
184  double alphaInv = 1/alpha;
185  double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
186  double v1 = v[0], v2 = v[1];
187 
188  J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
189  0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
190  0, 0, 1;
191  }
192  else {
193  J << 1,0, 0.5*v[1],
194  0,1, -0.5*v[0],
195  0,0, 1;
196  }
197  return J;
198 }
199 
200 /* ************************************************************************* */
202  return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
203 }
204 
205 /* ************************************************************************* */
206 // see doc/math.lyx, SE(2) section
208  OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
209  OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
210  OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
211  const Point2 q = r_.unrotate(point - t_, Hrotation, Hpoint);
212  if (Htranslation) *Htranslation << -1.0, 0.0, 0.0, -1.0;
213  return q;
214 }
215 
216 Matrix Pose2::transformTo(const Matrix& points) const {
217  if (points.rows() != 2) {
218  throw std::invalid_argument("Pose2:transformTo expects 2*N matrix.");
219  }
220  const Matrix2 Rt = rotation().transpose();
221  return Rt * (points.colwise() - t_); // Eigen broadcasting!
222 }
223 
224 /* ************************************************************************* */
225 // see doc/math.lyx, SE(2) section
227  OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
228  OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
229  OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
230  const Point2 q = r_.rotate(point, Hrotation, Hpoint);
231  if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix());
232  return q + t_;
233 }
234 
235 
236 Matrix Pose2::transformFrom(const Matrix& points) const {
237  if (points.rows() != 2) {
238  throw std::invalid_argument("Pose2:transformFrom expects 2*N matrix.");
239  }
240  const Matrix2 R = rotation().matrix();
241  return (R * points).colwise() + t_; // Eigen broadcasting!
242 }
243 
244 /* ************************************************************************* */
245 Rot2 Pose2::bearing(const Point2& point,
246  OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {
247  // make temporary matrices
248  Matrix23 D_d_pose; Matrix2 D_d_point;
249  Point2 d = transformTo(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0);
250  if (!Hpose && !Hpoint) return Rot2::relativeBearing(d);
251  Matrix12 D_result_d;
252  Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0);
253  if (Hpose) *Hpose = D_result_d * D_d_pose;
254  if (Hpoint) *Hpoint = D_result_d * D_d_point;
255  return result;
256 }
257 
258 /* ************************************************************************* */
259 Rot2 Pose2::bearing(const Pose2& pose,
260  OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 3> Hother) const {
261  Matrix12 D2;
262  Rot2 result = bearing(pose.t(), Hpose, Hother ? &D2 : 0);
263  if (Hother) {
264  Matrix12 H2_ = D2 * pose.r().matrix();
265  *Hother << H2_, Z_1x1;
266  }
267  return result;
268 }
269 /* ************************************************************************* */
270 double Pose2::range(const Point2& point,
271  OptionalJacobian<1,3> Hpose, OptionalJacobian<1,2> Hpoint) const {
272  Point2 d = point - t_;
273  if (!Hpose && !Hpoint) return d.norm();
274  Matrix12 D_r_d;
275  double r = norm2(d, D_r_d);
276  if (Hpose) {
277  Matrix23 D_d_pose;
278  D_d_pose << -r_.c(), r_.s(), 0.0,
279  -r_.s(), -r_.c(), 0.0;
280  *Hpose = D_r_d * D_d_pose;
281  }
282  if (Hpoint) *Hpoint = D_r_d;
283  return r;
284 }
285 
286 /* ************************************************************************* */
287 double Pose2::range(const Pose2& pose,
288  OptionalJacobian<1,3> Hpose,
289  OptionalJacobian<1,3> Hother) const {
290  Point2 d = pose.t() - t_;
291  if (!Hpose && !Hother) return d.norm();
292  Matrix12 D_r_d;
293  double r = norm2(d, D_r_d);
294  if (Hpose) {
295  Matrix23 D_d_pose;
296  D_d_pose <<
297  -r_.c(), r_.s(), 0.0,
298  -r_.s(), -r_.c(), 0.0;
299  *Hpose = D_r_d * D_d_pose;
300  }
301  if (Hother) {
302  Matrix23 D_d_other;
303  D_d_other <<
304  pose.r_.c(), -pose.r_.s(), 0.0,
305  pose.r_.s(), pose.r_.c(), 0.0;
306  *Hother = D_r_d * D_d_other;
307  }
308  return r;
309 }
310 
311 /* *************************************************************************
312  * Align finds the angle using a linear method:
313  * a = Pose2::transformFrom(b) = t + R*b
314  * We need to remove the centroids from the data to find the rotation
315  * using db=[dbx;dby] and a=[dax;day] we have
316  * |dax| |c -s| |dbx| |dbx -dby| |c|
317  * | | = | | * | | = | | * | | = H_i*cs
318  * |day| |s c| |dby| |dby dbx| |s|
319  * where the Hi are the 2*2 matrices. Then we will minimize the criterion
320  * J = \sum_i norm(a_i - H_i * cs)
321  * Taking the derivative with respect to cs and setting to zero we have
322  * cs = (\sum_i H_i' * a_i)/(\sum H_i'*H_i)
323  * The hessian is diagonal and just divides by a constant, but this
324  * normalization constant is irrelevant, since we take atan2.
325  * i.e., cos ~ sum(dbx*dax + dby*day) and sin ~ sum(-dby*dax + dbx*day)
326  * The translation is then found from the centroids
327  * as they also satisfy ca = t + R*cb, hence t = ca - R*cb
328  */
329 
330 std::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
331  const size_t n = ab_pairs.size();
332  if (n < 2) {
333  return {}; // we need at least 2 pairs
334  }
335 
336  // calculate centroids
337  Point2 ca(0, 0), cb(0, 0);
338  for (const Point2Pair& pair : ab_pairs) {
339  ca += pair.first;
340  cb += pair.second;
341  }
342  const double f = 1.0/n;
343  ca *= f;
344  cb *= f;
345 
346  // calculate cos and sin
347  double c = 0, s = 0;
348  for (const Point2Pair& pair : ab_pairs) {
349  Point2 da = pair.first - ca;
350  Point2 db = pair.second - cb;
351  c += db.x() * da.x() + db.y() * da.y();
352  s += -db.y() * da.x() + db.x() * da.y();
353  }
354 
355  // calculate angle and translation
356  const double theta = atan2(s, c);
357  const Rot2 R = Rot2::fromAngle(theta);
358  const Point2 t = ca - R*cb;
359  return Pose2(R, t);
360 }
361 
362 std::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
363  if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
364  throw std::invalid_argument(
365  "Pose2:Align expects 2*N matrices of equal shape.");
366  }
367  Point2Pairs ab_pairs;
368  for (Eigen::Index j = 0; j < a.cols(); j++) {
369  ab_pairs.emplace_back(a.col(j), b.col(j));
370  }
371  return Pose2::Align(ab_pairs);
372 }
373 
374 /* ************************************************************************* */
375 } // namespace gtsam
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
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
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
Pose2.h
2D Pose
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
Testable.h
Concept check for values that can be used in unit tests.
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Pose2::rotation
const Rot2 & rotation(OptionalJacobian< 1, 3 > Hself={}) const
rotation
Definition: Pose2.h:270
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::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
concepts.h
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:85
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
result
Values result
Definition: OdometryOptimize.cpp:8
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
gtsam::Point2Pair
std::pair< Point2, Point2 > Point2Pair
Definition: Point2.h:35
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
simple::R0
static Rot3 R0
Definition: testInitializePose3.cpp:48
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::Triplet< double >
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::Pose2::theta
double theta() const
get theta
Definition: Pose2.h:252
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::Rot2
Definition: Rot2.h:35
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_CONCEPT_POSE_INST
#define GTSAM_CONCEPT_POSE_INST(T)
Definition: geometry/concepts.h:73
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Pose2 &pose)
Definition: Pose2.cpp:55
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Pose2::x
double x() const
get x
Definition: Pose2.h:246
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v2
Vector v2
Definition: testSerializationBase.cpp:39
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::R_PI_2
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.))
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
setZero
v setZero(3)
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::internal::Align
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair &centroids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Definition: Similarity2.cpp:74
align_3::t
Point2 t(10, 10)
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27
gtsam::Pose2::y
double y() const
get y
Definition: Pose2.h:249
R
Rot2 R(Rot2::fromAngle(0.1))
v1
Vector v1
Definition: testSerializationBase.cpp:38
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


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24