00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef __SE3_H
00021 #define __SE3_H
00022
00023 #include <TooN/so3.h>
00024
00025 #ifndef TOON_NO_NAMESPACE
00026 namespace TooN {
00027 #endif
00028
00029 class SE3 {
00030 friend SE3 operator*(const SO3& lhs, const SE3& rhs);
00031 friend std::istream& operator>> (std::istream& is, SE3& rhs);
00032
00033 public:
00034 inline SE3();
00035 template <class A> inline SE3(const SO3& R, const FixedVector<3,A>& T) : my_rotation(R), my_translation(T) {}
00036
00037
00038 inline SO3& get_rotation(){return my_rotation;}
00039 inline const SO3& get_rotation() const {return my_rotation;}
00040 inline Vector<3>& get_translation() {return my_translation;}
00041 inline const Vector<3>& get_translation() const {return my_translation;}
00042
00043 static inline SE3 exp(const Vector<6>& vect);
00044 static inline Vector<6> ln(const SE3& se3);
00045 inline Vector<6> ln() const { return SE3::ln(*this); }
00046
00047 inline SE3 inverse() const;
00048
00049 inline SE3& operator *=(const SE3& rhs);
00050 inline SE3 operator *(const SE3& rhs) const { return SE3(my_rotation*rhs.my_rotation, my_translation + my_rotation*rhs.my_translation); }
00051 inline SE3& left_multiply_by(const SE3& left);
00052
00053 static inline Vector<4> generator_field(int i, Vector<4> pos);
00054
00055
00056 template<class Accessor>
00057 inline void adjoint(FixedVector<6,Accessor>& vect)const;
00058
00059 template<class Accessor>
00060 inline void trinvadjoint(FixedVector<6,Accessor>& vect)const;
00061
00062 template <class Accessor>
00063 inline void adjoint(FixedMatrix<6,6,Accessor>& M)const;
00064
00065 template <class Accessor>
00066 inline void trinvadjoint(FixedMatrix<6,6,Accessor>& M)const;
00067
00068
00069 template <class A1, class A2, class A3>
00070 Vector<2> project_transformed_point(const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const
00071 {
00072 const double z_inv = 1.0/in_frame[2];
00073 const double x_z_inv = in_frame[0]*z_inv;
00074 const double y_z_inv = in_frame[1]*z_inv;
00075 const double cross = x_z_inv * y_z_inv;
00076 J_pose[0][0] = J_pose[1][1] = z_inv;
00077 J_pose[0][1] = J_pose[1][0] = 0;
00078 J_pose[0][2] = -x_z_inv * z_inv;
00079 J_pose[1][2] = -y_z_inv * z_inv;
00080 J_pose[0][3] = -cross;
00081 J_pose[0][4] = 1 + x_z_inv*x_z_inv;
00082 J_pose[0][5] = -y_z_inv;
00083 J_pose[1][3] = -1 - y_z_inv*y_z_inv;
00084 J_pose[1][4] = cross;
00085 J_pose[1][5] = x_z_inv;
00086
00087 const TooN::Matrix<3>& R = get_rotation().get_matrix();
00088 J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00089 J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00090 J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00091 J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00092 J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00093 J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00094
00095 Vector<2> uv;
00096 uv[0] = x_z_inv;
00097 uv[1] = y_z_inv;
00098 return uv;
00099 }
00100
00101 template <class A> Vector<3>
00102 transform(const FixedVector<3,A>& x) const { return my_rotation * x + my_translation; }
00103
00104 template <class A1, class A2, class A3>
00105 Vector<3> transform(const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose) const {
00106 const Vector<3> se3_x = *this * x;
00107 J_x = my_rotation.get_matrix();
00108 Identity(J_pose.template slice<0,0,3,3>());
00109 J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
00110 J_pose[0][4] = se3_x[2]; J_pose[0][5] = -se3_x[1];
00111 J_pose[1][3] = -se3_x[2]; J_pose[1][5] = se3_x[0];
00112 J_pose[2][3] = se3_x[1]; J_pose[2][4] = -se3_x[0];
00113 return se3_x;
00114 }
00115
00116
00117 template <class A>
00118 Vector<3> transform_uvq(const FixedVector<3,A>& uvq) const {
00119 const Matrix<3>& R = my_rotation.get_matrix();
00120 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * my_translation;
00121 const double inv_z = 1.0/ DqT[2];
00122 const double vals[3] = {DqT[0] * inv_z, DqT[1]*inv_z, uvq[2]*inv_z};
00123 return Vector<3>(vals);
00124 }
00125
00126
00127 template <class A1, class A2, class A3>
00128 Vector<2> transform_and_project(const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose) const
00129 {
00130 return project_transformed_point(*this * x, J_x, J_pose);
00131 }
00132
00133 template <class A1, class A2, class A3>
00134 Vector<2> transform_and_project_uvq(const FixedVector<3,A1>& uvq, FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose) const
00135 {
00136 const Vector<3> DqT = get_rotation() * TooN::unproject(uvq.template slice<0,2>()) + uvq[2] * get_translation();
00137 const Vector<2> uv = project_transformed_point(DqT, J_uvq, J_pose);
00138 J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * get_translation();
00139 J_pose.template slice<0,0,2,3>() *= uvq[2];
00140 return uv;
00141 }
00142
00143 private:
00144 SO3 my_rotation;
00145 Vector<3> my_translation;
00146 };
00147
00148
00149
00150 inline SE3 operator*(const SO3& lhs, const SE3& rhs);
00151
00152
00153
00154
00155
00156 template<class Accessor>
00157 inline void SE3::adjoint(FixedVector<6,Accessor>& vect)const{
00158 vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
00159 vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
00160 vect.template slice<0,3>() += my_translation ^ vect.template slice<3,3>();
00161 }
00162
00163
00164
00165
00166 template<class Accessor>
00167 inline void SE3::trinvadjoint(FixedVector<6,Accessor>& vect)const{
00168 vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
00169 vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
00170 vect.template slice<3,3>() += my_translation ^ vect.template slice<0,3>();
00171 }
00172
00173 template <class Accessor>
00174 inline void SE3::adjoint(FixedMatrix<6,6,Accessor>& M)const{
00175 for(int i=0; i<6; i++){
00176 adjoint(M.T()[i]);
00177 }
00178 for(int i=0; i<6; i++){
00179 adjoint(M[i]);
00180 }
00181 }
00182
00183 template <class Accessor>
00184 inline void SE3::trinvadjoint(FixedMatrix<6,6,Accessor>& M)const{
00185 for(int i=0; i<6; i++){
00186 trinvadjoint(M.T()[i]);
00187 }
00188 for(int i=0; i<6; i++){
00189 trinvadjoint(M[i]);
00190 }
00191 }
00192
00193
00194
00195 inline std::ostream& operator <<(std::ostream& os, const SE3& rhs){
00196 for(int i=0; i<3; i++){
00197 os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] << std::endl;
00198 }
00199 return os;
00200 }
00201
00202
00203 inline std::istream& operator>>(std::istream& is, SE3& rhs){
00204 for(int i=0; i<3; i++){
00205 is >> rhs.get_rotation().my_matrix[i] >> rhs.get_translation()[i];
00206 }
00207 return is;
00208 }
00209
00210
00212
00213
00215
00216 template<class VectorType>
00217 struct SE3VMult {
00218 inline static void eval(Vector<4>& ret, const SE3& lhs, const VectorType& rhs){
00219 ret.template slice<0,3>()=lhs.get_rotation()*rhs.template slice<0,3>();
00220 ret.template slice<0,3>()+=lhs.get_translation() * rhs[3];
00221 ret[3] = rhs[3];
00222 }
00223 };
00224
00225 template<class Accessor> inline
00226 Vector<4> operator*(const SE3& lhs, const FixedVector<4,Accessor>& rhs){
00227 return Vector<4>(lhs,rhs,Operator<SE3VMult<FixedVector<4, Accessor> > >());
00228 }
00229
00230 template<class Accessor> inline
00231 Vector<4> operator*(const SE3& lhs, const DynamicVector<Accessor>& rhs){
00232
00233 return Vector<4>(lhs,rhs,Operator<SE3VMult<DynamicVector<Accessor> > >());
00234 }
00235
00236 template <class Accessor> inline
00237 Vector<3> operator*(const SE3& lhs, const FixedVector<3,Accessor>& rhs){
00238 return lhs.transform(rhs);
00239 }
00240
00241
00243
00244
00246
00247 template<class Accessor>
00248 struct VSE3Mult {
00249 inline static void eval(Vector<4>& ret, const FixedVector<4,Accessor>& lhs, const SE3& rhs){
00250 ret.template slice<0,3>() = lhs.template slice<0,3>() * rhs.get_rotation();
00251 ret[3] = lhs[3];
00252 ret[3] += lhs.template slice<0,3>() * rhs.get_translation();
00253 }
00254 };
00255
00256 template<class Accessor> inline
00257 Vector<4> operator*(const FixedVector<4,Accessor>& lhs, const SE3& rhs){
00258 return Vector<4>(lhs,rhs,Operator<VSE3Mult<Accessor> >());
00259 }
00260
00261
00262
00264
00265
00267
00268 template <int RHS, class Accessor>
00269 struct SE3MMult {
00270 inline static void eval(Matrix<4,RHS>& ret, const SE3& lhs, const FixedMatrix<4,RHS,Accessor>& rhs){
00271 for(int i=0; i<RHS; i++){
00272 ret.T()[i].template slice<0,3>() = lhs.get_rotation() * rhs.T()[i].template slice<0,3>();
00273 ret.T()[i].template slice<0,3>() += lhs.get_translation() * rhs(3,i);
00274 ret(3,i) = rhs(3,i);
00275 }
00276 }
00277 };
00278
00279
00280 template <int RHS, class Accessor> inline
00281 Matrix<4,RHS> operator*(const SE3& lhs, const FixedMatrix<4,RHS,Accessor>& rhs){
00282 return Matrix<4,RHS>(lhs,rhs,Operator<SE3MMult<RHS,Accessor> >());
00283 }
00284
00285
00287
00288
00290
00291 template <int LHS, class Accessor>
00292 struct MSE3Mult {
00293 inline static void eval(Matrix<LHS,4>& ret, const FixedMatrix<LHS,4,Accessor>& lhs, const SE3& rhs){
00294 for(int i=0; i<LHS; i++){
00295 ret[i].template slice<0,3>() = lhs[i].template slice<0,3>() * rhs.get_rotation();
00296 ret(i,3) = rhs.get_translation() * lhs[i].template slice<0,3>();
00297 ret(i,3) += lhs(i,3);
00298 }
00299 }
00300 };
00301
00302
00303 template <int LHS, class Accessor> inline
00304 Matrix<LHS,4> operator*(const FixedMatrix<LHS,4,Accessor>& lhs, const SE3& rhs){
00305 return Matrix<LHS,4>(lhs,rhs,Operator<MSE3Mult<LHS,Accessor> >());
00306 }
00307
00308
00309 namespace SE3static
00310 {
00311 static double zero[3]={0,0,0};
00312 }
00313
00314 inline SE3::SE3() :
00315 my_translation(SE3static::zero)
00316 {}
00317
00318
00319 inline SE3 SE3::exp(const Vector<6>& mu){
00320 static const double one_6th = 1.0/6.0;
00321 static const double one_20th = 1.0/20.0;
00322
00323 SE3 result;
00324
00325 const Vector<3> w = mu.slice<3,3>();
00326 const double theta_sq = w*w;
00327 const double theta = sqrt(theta_sq);
00328 double A, B;
00329
00330 const Vector<3> cross = w ^ mu.slice<0,3>();
00331 if (theta_sq < 1e-8) {
00332 A = 1.0 - one_6th * theta_sq;
00333 B = 0.5;
00334 result.get_translation() = mu.slice<0,3>() + 0.5 * cross;
00335 } else {
00336 double C;
00337 if (theta_sq < 1e-6) {
00338 C = one_6th*(1.0 - one_20th * theta_sq);
00339 A = 1.0 - theta_sq * C;
00340 B = 0.5 - 0.25 * one_6th * theta_sq;
00341 } else {
00342 const double inv_theta = 1.0/theta;
00343 A = sin(theta) * inv_theta;
00344 B = (1 - cos(theta)) * (inv_theta * inv_theta);
00345 C = (1 - A) * (inv_theta * inv_theta);
00346 }
00347 result.get_translation() = mu.slice<0,3>() + B * cross + C * (w ^ cross);
00348 }
00349 rodrigues_so3_exp(w, A, B, result.my_rotation.my_matrix);
00350 return result;
00351 }
00352
00353
00354 inline Vector<6> SE3::ln(const SE3& se3) {
00355 Vector<3> rot = se3.my_rotation.ln();
00356 double theta = sqrt(rot*rot);
00357 double shtot = 0.5;
00358
00359 if(theta > 0.00001) {
00360 shtot = sin(theta/2)/theta;
00361 }
00362
00363
00364 Vector<3> halfrot = rot * -0.5;
00365 SO3 halfrotator = SO3::exp(halfrot);
00366
00367 Vector<3> rottrans = halfrotator * se3.my_translation;
00368
00369 if(theta > 0.001){
00370 rottrans -= rot * ((se3.my_translation * rot) * (1-2*shtot) / (rot*rot));
00371 } else {
00372 rottrans -= rot * ((se3.my_translation * rot)/24);
00373 }
00374
00375 rottrans /= (2 * shtot);
00376
00377 Vector<6> result;
00378 result.slice<0,3>()=rottrans;
00379 result.slice<3,3>()=rot;
00380 return result;
00381 }
00382
00383 inline SE3 SE3::inverse() const {
00384 const SO3& rinv = my_rotation.inverse();
00385 return SE3(rinv, -(rinv*my_translation));
00386 }
00387
00388 inline SE3& SE3::left_multiply_by(const SE3& left) {
00389 my_translation = left.my_translation + left.get_rotation() * my_translation;
00390 my_rotation = left.my_rotation * my_rotation;
00391 return *this;
00392 }
00393
00394 inline Vector<4> SE3::generator_field(int i, Vector<4> pos){
00395 double result_d[]={0,0,0,0};
00396 Vector<4> result(result_d);
00397 if(i < 3){
00398 result[i]=pos[3];
00399 return result;
00400 }
00401 result[(i+1)%3] = - pos[(i+2)%3];
00402 result[(i+2)%3] = pos[(i+1)%3];
00403 return result;
00404 }
00405
00406
00407 inline SE3 operator*(const SO3& lhs, const SE3& rhs){
00408 SE3 result;
00409 result.my_rotation = lhs*rhs.my_rotation;
00410 result.my_translation = lhs*rhs.my_translation;
00411 return result;
00412 }
00413
00414 #ifndef TOON_NO_NAMESPACE
00415 }
00416 #endif
00417
00418 #endif