Curves
A library for curves generation.
Pose2_Expressions.hpp
Go to the documentation of this file.
1 /*
2  * @file Pose2_Expressions.hpp
3  * @date Nov 24, 2015
4  * @author Renaud DubĂ©, Abel Gawel
5  *
6  * This helper implements expressions for Pose2 objects in order to
7  * perform spherical linear interpolation (slerp) in SE2.
8  */
9 
10 #ifndef POSE2_EXPRESSIONS_HPP
11 #define POSE2_EXPRESSIONS_HPP
12 
13 #include "gtsam/nonlinear/Expression.h"
14 #include "gtsam/geometry/Pose2.h"
15 #include "gtsam/geometry/Point2.h"
16 
17 #define N_TEST_ITERATIONS 10000
18 
19 typedef gtsam::Pose2 SE2;
20 typedef gtsam::Expression<SE2> ETransformation;
21 typedef gtsam::OptionalJacobian<3, 3> ChartJacobian;
23 typedef gtsam::Expression<gtsam::Vector3> EVector3;
24 
25 inline SE2 makeRandomSE2() {
26  gtsam::Vector2 pval;
27  pval.setRandom();
28 
29  return SE2(((double)rand() / RAND_MAX)*6.2832, gtsam::Point2(pval));
30 }
31 
33  const SE2& T, ChartJacobian HT) {
34  return T.inverse(HT);
35 }
36 
38  const ETransformation& T) {
40 }
41 
42 inline SE2 composeImplementation(const SE2& T1, const SE2& T2,
43  ChartJacobian HT1, ChartJacobian HT2) {
44  return T1.compose(T2, HT1, HT2);
45 }
46 
48  return ETransformation(&composeImplementation, T1, T2);
49 }
50 
51 inline double function1(double alpha) {
52  double K = 0.0000330688 / 0.0833333;
53  double threshold = pow((std::numeric_limits<double>::epsilon()/K),0.25)*10;
54  double rval;
55  if (fabs(alpha) > threshold) {
56  rval = 1/alpha-0.5*sin(alpha)/(1-cos(alpha));
57  } else {
58  rval = 0.0833333 * alpha + 0.00138889 * pow(alpha,3);
59  }
60  return rval;
61 }
62 
63 inline double function2(double alpha) {
64  double K = 0.00138889;
65  double threshold = pow((std::numeric_limits<double>::epsilon()/K),0.25)*10;
66  double rval;
67  if (fabs(alpha) > threshold) {
68  rval = alpha*0.5*sin(alpha)/(1-cos(alpha));
69  } else {
70  rval = 1 - 0.0833333 * pow(alpha,2);
71  }
72  return rval;
73 }
74 
76  Vector3 v = gtsam::Pose2::Logmap(T, HT);
77  if (HT) {
78  double alpha = v[2];
79  gtsam::Matrix3 J;
80 
81  double alphaInv = 1/alpha;
82  double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
83  double v1 = v[0], v2 = v[1];
84  J << function2(alpha), -0.5*alpha, v1*function1(alpha) + 0.5*v2,
85  0.5*alpha, function2(alpha), v2*function1(alpha) - 0.5*v1,
86  0, 0, 1;
87 
88  *HT = J;
89  }
90  return v;
91 }
92 
95 }
96 
98  ChartJacobian Hp) {
99  return gtsam::Pose2::Expmap(params, Hp);
100 }
101 
104 }
105 
106 template <int N>
107 Eigen::Matrix<double, N, 1> vectorScalingImplementation(const Eigen::Matrix<double, N, 1> & v, double alpha,
108  gtsam::OptionalJacobian<N, N> H1,
109  gtsam::OptionalJacobian<N, 1> H2) {
110  if (H1) {
111  *H1 = gtsam::OptionalJacobian<N,N>::Jacobian::Identity()*alpha;
112  }
113  if (H2) {
114  *H2 = v;
115  }
116  return v*alpha;
117 }
118 
119 template <int N>
120 gtsam::Expression<Eigen::Matrix<double, N, 1> > vectorScaling(const gtsam::Expression<Eigen::Matrix<double, N, 1> >& v, double alpha) {
121  return gtsam::Expression<Eigen::Matrix<double, N, 1> >(boost::bind(&vectorScalingImplementation<N>, _1, alpha, _2, boost::none), v);
122 }
123 
125  const ETransformation& T0,
126  const ETransformation& T1,
127  double alpha) {
129 }
130 
131 inline gtsam::Point2 transformFromCurveImplementation(gtsam::Pose2 pose, gtsam::Point2 point,
132  gtsam::OptionalJacobian<2, 3> H1,
133  gtsam::OptionalJacobian<2, 2> H2) {
134  return pose.transform_from(point, H1, H2);
135 }
136 
137 inline gtsam::Expression<gtsam::Point2> transformFromCurve(const gtsam::Expression<gtsam::Pose2>& Epose,
138  const gtsam::Expression<gtsam::Point2>& Epoint) {
139  return gtsam::Expression<gtsam::Point2>(&transformFromCurveImplementation,
140  Epose, Epoint);
141 }
142 
143 inline double distanceBetweenPointsImplementation(gtsam::Point2 p1, gtsam::Point2 p2,
144  gtsam::OptionalJacobian<1, 2> H1,
145  gtsam::OptionalJacobian<1, 2> H2) {
146  return p1.distance(p2, H1, H2);
147 }
148 
149 inline gtsam::Expression<double> distanceBetweenPoints(const gtsam::Expression<gtsam::Point2>& Ep1,
150  const gtsam::Expression<gtsam::Point2>& Ep2) {
151  return gtsam::Expression<double>(&distanceBetweenPointsImplementation,
152  Ep1, Ep2);
153 }
154 
155 inline gtsam::Point2 pointsSubtractionImplementation(gtsam::Point2 p1, gtsam::Point2 p2,
156  gtsam::OptionalJacobian<2, 2> H1,
157  gtsam::OptionalJacobian<2, 2> H2) {
158  if (H1) {
159  *H1 << 1, 0, 0, 1;
160  }
161 
162  if (H2) {
163  *H2 << -1, 0, 0, -1;
164  }
165 
166  return gtsam::Point2(p1.x()-p2.x(),p1.y()-p2.y());
167 }
168 
169 inline gtsam::Expression<gtsam::Point2> pointsSubtraction(const gtsam::Expression<gtsam::Point2>& Ep1,
170  const gtsam::Expression<gtsam::Point2>& Ep2) {
171  return gtsam::Expression<gtsam::Point2>(&pointsSubtractionImplementation,
172  Ep1, Ep2);
173 }
174 
175 inline gtsam::Point2 pose2AsPoint2Implementation(gtsam::Pose2 pose, gtsam::OptionalJacobian<2, 3> H) {
176  if (H) {
177  *H << 1, 0, 0, 0, 1, 0;
178  }
179  return gtsam::Point2(pose.x(), pose.y());
180 }
181 
182 inline gtsam::Expression<gtsam::Point2> pose2AsPoint2(const gtsam::Expression<gtsam::Pose2>& Epose) {
183  return gtsam::Expression<gtsam::Point2>(&pose2AsPoint2Implementation, Epose);
184 }
185 
186 inline double poseRangeImplementation(const SE2& T1, const SE2& T2,
187  gtsam::OptionalJacobian<1, 3> H1,
188  gtsam::OptionalJacobian<1, 3> H2) {
189  return T1.range(T2, H1, H2);
190 }
191 
192 inline gtsam::Expression<double> poseRange(const gtsam::Expression<gtsam::Pose2>& Epose1,
193  const gtsam::Expression<gtsam::Pose2>& Epose2) {
194  return gtsam::Expression<double>(&poseRangeImplementation, Epose1, Epose2);
195 }
196 
197 inline double pointRangeImplementation(const SE2& pose, const gtsam::Point2& point,
198  gtsam::OptionalJacobian<1, 3> H1,
199  gtsam::OptionalJacobian<1, 2> H2) {
200  return pose.range(point, H1, H2);
201 }
202 
203 inline gtsam::Expression<double> pointRange(const gtsam::Expression<gtsam::Pose2>& Epose,
204  const gtsam::Expression<gtsam::Point2>& Epoint) {
205  return gtsam::Expression<double>(&pointRangeImplementation, Epose, Epoint);
206 }
207 
208 #endif /* POSE2_EXPRESSIONS_HPP */
SE2 composeImplementation(const SE2 &T1, const SE2 &T2, ChartJacobian HT1, ChartJacobian HT2)
gtsam::OptionalJacobian< 3, 3 > ChartJacobian
ETransformation transformationExp(const EVector3 &params)
gtsam::Vector3 Vector3
SE2 makeRandomSE2()
gtsam::Point2 transformFromCurveImplementation(gtsam::Pose2 pose, gtsam::Point2 point, gtsam::OptionalJacobian< 2, 3 > H1, gtsam::OptionalJacobian< 2, 2 > H2)
double pointRangeImplementation(const SE2 &pose, const gtsam::Point2 &point, gtsam::OptionalJacobian< 1, 3 > H1, gtsam::OptionalJacobian< 1, 2 > H2)
ETransformation inverse(const ETransformation &T)
gtsam::Expression< double > pointRange(const gtsam::Expression< gtsam::Pose2 > &Epose, const gtsam::Expression< gtsam::Point2 > &Epoint)
double poseRangeImplementation(const SE2 &T1, const SE2 &T2, gtsam::OptionalJacobian< 1, 3 > H1, gtsam::OptionalJacobian< 1, 3 > H2)
gtsam::Point2 pointsSubtractionImplementation(gtsam::Point2 p1, gtsam::Point2 p2, gtsam::OptionalJacobian< 2, 2 > H1, gtsam::OptionalJacobian< 2, 2 > H2)
double distanceBetweenPointsImplementation(gtsam::Point2 p1, gtsam::Point2 p2, gtsam::OptionalJacobian< 1, 2 > H1, gtsam::OptionalJacobian< 1, 2 > H2)
SE2 inverseImplementation(const SE2 &T, ChartJacobian HT)
gtsam::Expression< double > poseRange(const gtsam::Expression< gtsam::Pose2 > &Epose1, const gtsam::Expression< gtsam::Pose2 > &Epose2)
SE2 transformationExpImplementation(const Vector3 &params, ChartJacobian Hp)
Vector3 transformationLogImplementation(const SE2 &T, ChartJacobian HT)
ETransformation compose(const ETransformation &T1, const ETransformation &T2)
Eigen::Matrix< double, N, 1 > vectorScalingImplementation(const Eigen::Matrix< double, N, 1 > &v, double alpha, gtsam::OptionalJacobian< N, N > H1, gtsam::OptionalJacobian< N, 1 > H2)
gtsam::Expression< double > distanceBetweenPoints(const gtsam::Expression< gtsam::Point2 > &Ep1, const gtsam::Expression< gtsam::Point2 > &Ep2)
gtsam::Expression< SE2 > ETransformation
gtsam::Expression< gtsam::Vector3 > EVector3
gtsam::Expression< gtsam::Point2 > pointsSubtraction(const gtsam::Expression< gtsam::Point2 > &Ep1, const gtsam::Expression< gtsam::Point2 > &Ep2)
gtsam::Expression< gtsam::Point2 > transformFromCurve(const gtsam::Expression< gtsam::Pose2 > &Epose, const gtsam::Expression< gtsam::Point2 > &Epoint)
gtsam::Expression< gtsam::Point2 > pose2AsPoint2(const gtsam::Expression< gtsam::Pose2 > &Epose)
gtsam::Point2 pose2AsPoint2Implementation(gtsam::Pose2 pose, gtsam::OptionalJacobian< 2, 3 > H)
gtsam::Expression< Eigen::Matrix< double, N, 1 > > vectorScaling(const gtsam::Expression< Eigen::Matrix< double, N, 1 > > &v, double alpha)
double function2(double alpha)
ETransformation slerp(const ETransformation &T0, const ETransformation &T1, double alpha)
gtsam::Pose2 SE2
EVector3 transformationLog(const ETransformation &T)
double function1(double alpha)