00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #ifndef TOON_INCLUDE_SE2_H
00034 #define TOON_INCLUDE_SE2_H
00035
00036 #include <TooN/so2.h>
00037
00038
00039 namespace TooN {
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 template <typename Precision = double>
00052 class SE2 {
00053 public:
00054
00055 SE2() : my_translation(Zeros) {}
00056 template <class A> SE2(const SO2<Precision>& R, const Vector<2,Precision,A>& T) : my_rotation(R), my_translation(T) {}
00057 template <int S, class P, class A> SE2(const Vector<S, P, A> & v) { *this = exp(v); }
00058
00059
00060 SO2<Precision> & get_rotation(){return my_rotation;}
00061
00062 const SO2<Precision> & get_rotation() const {return my_rotation;}
00063
00064 Vector<2, Precision> & get_translation() {return my_translation;}
00065
00066 const Vector<2, Precision> & get_translation() const {return my_translation;}
00067
00068
00069
00070
00071 template <int S, typename P, typename A>
00072 static inline SE2 exp(const Vector<S,P, A>& vect);
00073
00074
00075
00076 static inline Vector<3, Precision> ln(const SE2& se2);
00077
00078 Vector<3, Precision> ln() const { return SE2::ln(*this); }
00079
00080
00081 SE2 inverse() const {
00082 const SO2<Precision> & rinv = my_rotation.inverse();
00083 return SE2(rinv, -(rinv*my_translation));
00084 };
00085
00086
00087
00088 template <typename P>
00089 SE2<typename Internal::MultiplyType<Precision,P>::type> operator *(const SE2<P>& rhs) const {
00090 return SE2<typename Internal::MultiplyType<Precision,P>::type>(my_rotation*rhs.get_rotation(), my_translation + my_rotation*rhs.get_translation());
00091 }
00092
00093
00094
00095 inline SE2& operator *=(const SE2& rhs) {
00096 *this = *this * rhs;
00097 return *this;
00098 }
00099
00100
00101
00102
00103
00104
00105 static inline Matrix<3,3, Precision> generator(int i) {
00106 Matrix<3,3,Precision> result(Zeros);
00107 if(i < 2){
00108 result[i][2] = 1;
00109 return result;
00110 }
00111 result[0][1] = -1;
00112 result[1][0] = 1;
00113 return result;
00114 }
00115
00116
00117
00118 template<typename Accessor>
00119 Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor> & vect) const {
00120 Vector<3, Precision> result;
00121 result[2] = vect[2];
00122 result.template slice<0,2>() = my_rotation * vect.template slice<0,2>();
00123 result[0] += vect[2] * my_translation[1];
00124 result[1] -= vect[2] * my_translation[0];
00125 return result;
00126 }
00127
00128 template <typename Accessor>
00129 Matrix<3,3,Precision> adjoint(const Matrix<3,3,Precision,Accessor>& M) const {
00130 Matrix<3,3,Precision> result;
00131 for(int i=0; i<3; ++i)
00132 result.T()[i] = adjoint(M.T()[i]);
00133 for(int i=0; i<3; ++i)
00134 result[i] = adjoint(result[i]);
00135 return result;
00136 }
00137
00138 private:
00139 SO2<Precision> my_rotation;
00140 Vector<2, Precision> my_translation;
00141 };
00142
00143
00144
00145 template <class Precision>
00146 inline std::ostream& operator<<(std::ostream& os, const SE2<Precision> & rhs){
00147 std::streamsize fw = os.width();
00148 for(int i=0; i<2; i++){
00149 os.width(fw);
00150 os << rhs.get_rotation().get_matrix()[i];
00151 os.width(fw);
00152 os << rhs.get_translation()[i] << '\n';
00153 }
00154 return os;
00155 }
00156
00157
00158
00159 template <class Precision>
00160 inline std::istream& operator>>(std::istream& is, SE2<Precision>& rhs){
00161 for(int i=0; i<2; i++)
00162 is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
00163 rhs.get_rotation().coerce();
00164 return is;
00165 }
00166
00167
00168
00169
00170
00171
00172
00173 namespace Internal {
00174 template<int S, typename P, typename PV, typename A>
00175 struct SE2VMult;
00176 }
00177
00178 template<int S, typename P, typename PV, typename A>
00179 struct Operator<Internal::SE2VMult<S,P,PV,A> > {
00180 const SE2<P> & lhs;
00181 const Vector<S,PV,A> & rhs;
00182
00183 Operator(const SE2<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
00184
00185 template <int S0, typename P0, typename A0>
00186 void eval(Vector<S0, P0, A0> & res ) const {
00187 SizeMismatch<3,S>::test(3, rhs.size());
00188 res.template slice<0,2>()=lhs.get_rotation()*rhs.template slice<0,2>();
00189 res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
00190 res[2] = rhs[2];
00191 }
00192 int size() const { return 3; }
00193 };
00194
00195
00196
00197 template<int S, typename P, typename PV, typename A>
00198 inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P> & lhs, const Vector<S,PV,A>& rhs){
00199 return Vector<3, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE2VMult<S,P,PV,A> >(lhs,rhs));
00200 }
00201
00202
00203
00204 template <typename P, typename PV, typename A>
00205 inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P>& lhs, const Vector<2,PV,A>& rhs){
00206 return lhs.get_translation() + lhs.get_rotation() * rhs;
00207 }
00208
00209
00210
00211
00212
00213
00214 namespace Internal {
00215 template<int S, typename P, typename PV, typename A>
00216 struct VSE2Mult;
00217 }
00218
00219 template<int S, typename P, typename PV, typename A>
00220 struct Operator<Internal::VSE2Mult<S,P,PV,A> > {
00221 const Vector<S,PV,A> & lhs;
00222 const SE2<P> & rhs;
00223
00224 Operator(const Vector<S,PV,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
00225
00226 template <int S0, typename P0, typename A0>
00227 void eval(Vector<S0, P0, A0> & res ) const {
00228 SizeMismatch<3,S>::test(3, lhs.size());
00229 res.template slice<0,2>() = lhs.template slice<0,2>()*rhs.get_rotation();
00230 res[2] = lhs[2];
00231 res[2] += lhs.template slice<0,2>() * rhs.get_translation();
00232 }
00233 int size() const { return 3; }
00234 };
00235
00236
00237
00238 template<int S, typename P, typename PV, typename A>
00239 inline Vector<3, typename Internal::MultiplyType<PV,P>::type> operator*(const Vector<S,PV,A>& lhs, const SE2<P> & rhs){
00240 return Vector<3, typename Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSE2Mult<S, P,PV,A> >(lhs,rhs));
00241 }
00242
00243
00244
00245
00246
00247
00248 namespace Internal {
00249 template <int R, int C, typename PM, typename A, typename P>
00250 struct SE2MMult;
00251 }
00252
00253 template<int R, int Cols, typename PM, typename A, typename P>
00254 struct Operator<Internal::SE2MMult<R, Cols, PM, A, P> > {
00255 const SE2<P> & lhs;
00256 const Matrix<R,Cols,PM,A> & rhs;
00257
00258 Operator(const SE2<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
00259
00260 template <int R0, int C0, typename P0, typename A0>
00261 void eval(Matrix<R0, C0, P0, A0> & res ) const {
00262 SizeMismatch<3,R>::test(3, rhs.num_rows());
00263 for(int i=0; i<rhs.num_cols(); ++i)
00264 res.T()[i] = lhs * rhs.T()[i];
00265 }
00266 int num_cols() const { return rhs.num_cols(); }
00267 int num_rows() const { return 3; }
00268 };
00269
00270
00271
00272 template <int R, int Cols, typename PM, typename A, typename P>
00273 inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
00274 return Matrix<3,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE2MMult<R, Cols, PM, A, P> >(lhs,rhs));
00275 }
00276
00277
00278
00279
00280
00281
00282 namespace Internal {
00283 template <int Rows, int C, typename PM, typename A, typename P>
00284 struct MSE2Mult;
00285 }
00286
00287 template<int Rows, int C, typename PM, typename A, typename P>
00288 struct Operator<Internal::MSE2Mult<Rows, C, PM, A, P> > {
00289 const Matrix<Rows,C,PM,A> & lhs;
00290 const SE2<P> & rhs;
00291
00292 Operator( const Matrix<Rows,C,PM,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
00293
00294 template <int R0, int C0, typename P0, typename A0>
00295 void eval(Matrix<R0, C0, P0, A0> & res ) const {
00296 SizeMismatch<3, C>::test(3, lhs.num_cols());
00297 for(int i=0; i<lhs.num_rows(); ++i)
00298 res[i] = lhs[i] * rhs;
00299 }
00300 int num_cols() const { return 3; }
00301 int num_rows() const { return lhs.num_rows(); }
00302 };
00303
00304
00305
00306 template <int Rows, int C, typename PM, typename A, typename P>
00307 inline Matrix<Rows,3, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE2<P> & rhs ){
00308 return Matrix<Rows,3,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE2Mult<Rows, C, PM, A, P> >(lhs,rhs));
00309 }
00310
00311 template <typename Precision>
00312 template <int S, typename PV, typename Accessor>
00313 inline SE2<Precision> SE2<Precision>::exp(const Vector<S, PV, Accessor>& mu)
00314 {
00315 SizeMismatch<3,S>::test(3, mu.size());
00316
00317 static const Precision one_6th = 1.0/6.0;
00318 static const Precision one_20th = 1.0/20.0;
00319
00320 SE2<Precision> result;
00321
00322 const Precision theta = mu[2];
00323 const Precision theta_sq = theta * theta;
00324
00325 const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta * mu[0]);
00326 result.get_rotation() = SO2<Precision>::exp(theta);
00327
00328 if (theta_sq < 1e-8){
00329 result.get_translation() = mu.template slice<0,2>() + 0.5 * cross;
00330 } else {
00331 Precision A, B;
00332 if (theta_sq < 1e-6) {
00333 A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
00334 B = 0.5 - 0.25 * one_6th * theta_sq;
00335 } else {
00336 const Precision inv_theta = (1.0/theta);
00337 const Precision sine = result.my_rotation.get_matrix()[1][0];
00338 const Precision cosine = result.my_rotation.get_matrix()[0][0];
00339 A = sine * inv_theta;
00340 B = (1 - cosine) * (inv_theta * inv_theta);
00341 }
00342 result.get_translation() = TooN::operator*(A,mu.template slice<0,2>()) + TooN::operator*(B,cross);
00343 }
00344 return result;
00345 }
00346
00347 template <typename Precision>
00348 inline Vector<3, Precision> SE2<Precision>::ln(const SE2<Precision> & se2) {
00349 const Precision theta = se2.get_rotation().ln();
00350
00351 Precision shtot = 0.5;
00352 if(fabs(theta) > 0.00001)
00353 shtot = sin(theta/2)/theta;
00354
00355 const SO2<Precision> halfrotator(theta * -0.5);
00356 Vector<3, Precision> result;
00357 result.template slice<0,2>() = (halfrotator * se2.get_translation())/(2 * shtot);
00358 result[2] = theta;
00359 return result;
00360 }
00361
00362
00363
00364
00365 template <typename Precision>
00366 inline SE2<Precision> operator*(const SO2<Precision> & lhs, const SE2<Precision>& rhs){
00367 return SE2<Precision>( lhs*rhs.get_rotation(), lhs*rhs.get_translation());
00368 }
00369
00370 }
00371 #endif