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 #ifndef TOON_INCLUDE_SE3_H
00031 #define TOON_INCLUDE_SE3_H
00032
00033 #include <TooN/so3.h>
00034
00035 namespace TooN {
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 template <typename Precision = double>
00050 class SE3 {
00051 public:
00052
00053 inline SE3() : my_translation(Zeros) {}
00054
00055 template <int S, typename P, typename A>
00056 SE3(const SO3<Precision> & R, const Vector<S, P, A>& T) : my_rotation(R), my_translation(T) {}
00057 template <int S, typename P, typename A>
00058 SE3(const Vector<S, P, A> & v) { *this = exp(v); }
00059
00060 template <class IP, int S, typename P, typename A>
00061 SE3(const Operator<Internal::Identity<IP> >&, const Vector<S, P, A>& T) : my_translation(T) {}
00062
00063
00064 inline SO3<Precision>& get_rotation(){return my_rotation;}
00065
00066 inline const SO3<Precision>& get_rotation() const {return my_rotation;}
00067
00068
00069 inline Vector<3, Precision>& get_translation() {return my_translation;}
00070
00071 inline const Vector<3, Precision>& get_translation() const {return my_translation;}
00072
00073
00074
00075
00076 template <int S, typename P, typename A>
00077 static inline SE3 exp(const Vector<S, P, A>& vect);
00078
00079
00080
00081
00082 static inline Vector<6, Precision> ln(const SE3& se3);
00083
00084 inline Vector<6, Precision> ln() const { return SE3::ln(*this); }
00085
00086 inline SE3 inverse() const {
00087 const SO3<Precision> rinv = get_rotation().inverse();
00088 return SE3(rinv, -(rinv*my_translation));
00089 }
00090
00091
00092
00093 inline SE3& operator *=(const SE3& rhs) {
00094 get_translation() += get_rotation() * rhs.get_translation();
00095 get_rotation() *= rhs.get_rotation();
00096 return *this;
00097 }
00098
00099
00100
00101 template<typename P>
00102 inline SE3<typename Internal::MultiplyType<Precision, P>::type> operator *(const SE3<P>& rhs) const {
00103 return SE3<typename Internal::MultiplyType<Precision, P>::type>(get_rotation()*rhs.get_rotation(), get_translation() + get_rotation()*rhs.get_translation());
00104 }
00105
00106 inline SE3& left_multiply_by(const SE3& left) {
00107 get_translation() = left.get_translation() + left.get_rotation() * get_translation();
00108 get_rotation() = left.get_rotation() * get_rotation();
00109 return *this;
00110 }
00111
00112 static inline Matrix<4,4,Precision> generator(int i){
00113 Matrix<4,4,Precision> result(Zeros);
00114 if(i < 3){
00115 result[i][3]=1;
00116 return result;
00117 }
00118 result[(i+1)%3][(i+2)%3] = -1;
00119 result[(i+2)%3][(i+1)%3] = 1;
00120 return result;
00121 }
00122
00123
00124 template<typename Base>
00125 inline static Vector<4,Precision> generator_field(int i, const Vector<4, Precision, Base>& pos)
00126 {
00127 Vector<4, Precision> result(Zeros);
00128 if(i < 3){
00129 result[i]=pos[3];
00130 return result;
00131 }
00132 result[(i+1)%3] = - pos[(i+2)%3];
00133 result[(i+2)%3] = pos[(i+1)%3];
00134 return result;
00135 }
00136
00137
00138
00139
00140
00141
00142 template<int S, typename P2, typename Accessor>
00143 inline Vector<6, Precision> adjoint(const Vector<S,P2, Accessor>& vect)const;
00144
00145
00146
00147 template<int S, typename P2, typename Accessor>
00148 inline Vector<6, Precision> trinvadjoint(const Vector<S,P2,Accessor>& vect)const;
00149
00150
00151 template <int R, int C, typename P2, typename Accessor>
00152 inline Matrix<6,6,Precision> adjoint(const Matrix<R,C,P2,Accessor>& M)const;
00153
00154
00155 template <int R, int C, typename P2, typename Accessor>
00156 inline Matrix<6,6,Precision> trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const;
00157
00158 private:
00159 SO3<Precision> my_rotation;
00160 Vector<3, Precision> my_translation;
00161 };
00162
00163
00164
00165
00166 template<typename Precision>
00167 template<int S, typename P2, typename Accessor>
00168 inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,P2, Accessor>& vect) const{
00169 SizeMismatch<6,S>::test(6, vect.size());
00170 Vector<6, Precision> result;
00171 result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00172 result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00173 result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
00174 return result;
00175 }
00176
00177
00178
00179
00180 template<typename Precision>
00181 template<int S, typename P2, typename Accessor>
00182 inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const Vector<S,P2, Accessor>& vect) const{
00183 SizeMismatch<6,S>::test(6, vect.size());
00184 Vector<6, Precision> result;
00185 result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
00186 result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
00187 result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
00188 return result;
00189 }
00190
00191 template<typename Precision>
00192 template<int R, int C, typename P2, typename Accessor>
00193 inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const Matrix<R,C,P2,Accessor>& M)const{
00194 SizeMismatch<6,R>::test(6, M.num_cols());
00195 SizeMismatch<6,C>::test(6, M.num_rows());
00196
00197 Matrix<6,6,Precision> result;
00198 for(int i=0; i<6; i++){
00199 result.T()[i] = adjoint(M.T()[i]);
00200 }
00201 for(int i=0; i<6; i++){
00202 result[i] = adjoint(result[i]);
00203 }
00204 return result;
00205 }
00206
00207 template<typename Precision>
00208 template<int R, int C, typename P2, typename Accessor>
00209 inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const{
00210 SizeMismatch<6,R>::test(6, M.num_cols());
00211 SizeMismatch<6,C>::test(6, M.num_rows());
00212
00213 Matrix<6,6,Precision> result;
00214 for(int i=0; i<6; i++){
00215 result.T()[i] = trinvadjoint(M.T()[i]);
00216 }
00217 for(int i=0; i<6; i++){
00218 result[i] = trinvadjoint(result[i]);
00219 }
00220 return result;
00221 }
00222
00223
00224
00225 template <typename Precision>
00226 inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
00227 std::streamsize fw = os.width();
00228 for(int i=0; i<3; i++){
00229 os.width(fw);
00230 os << rhs.get_rotation().get_matrix()[i];
00231 os.width(fw);
00232 os << rhs.get_translation()[i] << '\n';
00233 }
00234 return os;
00235 }
00236
00237
00238
00239
00240 template <typename Precision>
00241 inline std::istream& operator>>(std::istream& is, SE3<Precision>& rhs){
00242 for(int i=0; i<3; i++){
00243 is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
00244 }
00245 rhs.get_rotation().coerce();
00246 return is;
00247 }
00248
00249
00250
00251
00252
00253
00254 namespace Internal {
00255 template<int S, typename PV, typename A, typename P>
00256 struct SE3VMult;
00257 }
00258
00259 template<int S, typename PV, typename A, typename P>
00260 struct Operator<Internal::SE3VMult<S,PV,A,P> > {
00261 const SE3<P> & lhs;
00262 const Vector<S,PV,A> & rhs;
00263
00264 Operator(const SE3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
00265
00266 template <int S0, typename P0, typename A0>
00267 void eval(Vector<S0, P0, A0> & res ) const {
00268 SizeMismatch<4,S>::test(4, rhs.size());
00269 res.template slice<0,3>()=lhs.get_rotation() * rhs.template slice<0,3>();
00270 res.template slice<0,3>()+=lhs.get_translation() * rhs[3];
00271 res[3] = rhs[3];
00272 }
00273 int size() const { return 4; }
00274 };
00275
00276
00277
00278 template<int S, typename PV, typename A, typename P> inline
00279 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P> & lhs, const Vector<S,PV,A>& rhs){
00280 return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE3VMult<S,PV,A,P> >(lhs,rhs));
00281 }
00282
00283
00284
00285 template <typename PV, typename A, typename P> inline
00286 Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>& lhs, const Vector<3,PV,A>& rhs){
00287 return lhs.get_translation() + lhs.get_rotation() * rhs;
00288 }
00289
00290
00291
00292
00293
00294
00295 namespace Internal {
00296 template<int S, typename PV, typename A, typename P>
00297 struct VSE3Mult;
00298 }
00299
00300 template<int S, typename PV, typename A, typename P>
00301 struct Operator<Internal::VSE3Mult<S,PV,A,P> > {
00302 const Vector<S,PV,A> & lhs;
00303 const SE3<P> & rhs;
00304
00305 Operator( const Vector<S,PV,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00306
00307 template <int S0, typename P0, typename A0>
00308 void eval(Vector<S0, P0, A0> & res ) const {
00309 SizeMismatch<4,S>::test(4, lhs.size());
00310 res.template slice<0,3>()=lhs.template slice<0,3>() * rhs.get_rotation();
00311 res[3] = lhs[3];
00312 res[3] += lhs.template slice<0,3>() * rhs.get_translation();
00313 }
00314 int size() const { return 4; }
00315 };
00316
00317
00318
00319 template<int S, typename PV, typename A, typename P> inline
00320 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const Vector<S,PV,A>& lhs, const SE3<P> & rhs){
00321 return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSE3Mult<S,PV,A,P> >(lhs,rhs));
00322 }
00323
00324
00325
00326
00327
00328
00329 namespace Internal {
00330 template <int R, int C, typename PM, typename A, typename P>
00331 struct SE3MMult;
00332 }
00333
00334 template<int R, int Cols, typename PM, typename A, typename P>
00335 struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
00336 const SE3<P> & lhs;
00337 const Matrix<R,Cols,PM,A> & rhs;
00338
00339 Operator(const SE3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
00340
00341 template <int R0, int C0, typename P0, typename A0>
00342 void eval(Matrix<R0, C0, P0, A0> & res ) const {
00343 SizeMismatch<4,R>::test(4, rhs.num_rows());
00344 for(int i=0; i<rhs.num_cols(); ++i)
00345 res.T()[i] = lhs * rhs.T()[i];
00346 }
00347 int num_cols() const { return rhs.num_cols(); }
00348 int num_rows() const { return 4; }
00349 };
00350
00351
00352
00353 template <int R, int Cols, typename PM, typename A, typename P> inline
00354 Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
00355 return Matrix<4,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE3MMult<R, Cols, PM, A, P> >(lhs,rhs));
00356 }
00357
00358
00359
00360
00361
00362
00363 namespace Internal {
00364 template <int Rows, int C, typename PM, typename A, typename P>
00365 struct MSE3Mult;
00366 }
00367
00368 template<int Rows, int C, typename PM, typename A, typename P>
00369 struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
00370 const Matrix<Rows,C,PM,A> & lhs;
00371 const SE3<P> & rhs;
00372
00373 Operator( const Matrix<Rows,C,PM,A> & l, const SE3<P> & r ) : lhs(l), rhs(r) {}
00374
00375 template <int R0, int C0, typename P0, typename A0>
00376 void eval(Matrix<R0, C0, P0, A0> & res ) const {
00377 SizeMismatch<4, C>::test(4, lhs.num_cols());
00378 for(int i=0; i<lhs.num_rows(); ++i)
00379 res[i] = lhs[i] * rhs;
00380 }
00381 int num_cols() const { return 4; }
00382 int num_rows() const { return lhs.num_rows(); }
00383 };
00384
00385
00386
00387 template <int Rows, int C, typename PM, typename A, typename P> inline
00388 Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE3<P> & rhs ){
00389 return Matrix<Rows,4,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE3Mult<Rows, C, PM, A, P> >(lhs,rhs));
00390 }
00391
00392 template <typename Precision>
00393 template <int S, typename P, typename VA>
00394 inline SE3<Precision> SE3<Precision>::exp(const Vector<S, P, VA>& mu){
00395 SizeMismatch<6,S>::test(6, mu.size());
00396 static const Precision one_6th = 1.0/6.0;
00397 static const Precision one_20th = 1.0/20.0;
00398
00399 SE3<Precision> result;
00400
00401 const Vector<3,Precision> w = mu.template slice<3,3>();
00402 const Precision theta_sq = w*w;
00403 const Precision theta = sqrt(theta_sq);
00404 Precision A, B;
00405
00406 const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
00407 if (theta_sq < 1e-8) {
00408 A = 1.0 - one_6th * theta_sq;
00409 B = 0.5;
00410 result.get_translation() = mu.template slice<0,3>() + 0.5 * cross;
00411 } else {
00412 Precision C;
00413 if (theta_sq < 1e-6) {
00414 C = one_6th*(1.0 - one_20th * theta_sq);
00415 A = 1.0 - theta_sq * C;
00416 B = 0.5 - 0.25 * one_6th * theta_sq;
00417 } else {
00418 const Precision inv_theta = 1.0/theta;
00419 A = sin(theta) * inv_theta;
00420 B = (1 - cos(theta)) * (inv_theta * inv_theta);
00421 C = (1 - A) * (inv_theta * inv_theta);
00422 }
00423 result.get_translation() = mu.template slice<0,3>() + TooN::operator*(B, cross) + TooN::operator*(C, (w ^ cross));
00424 }
00425 rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
00426 return result;
00427 }
00428
00429 template <typename Precision>
00430 inline Vector<6, Precision> SE3<Precision>::ln(const SE3<Precision>& se3) {
00431 Vector<3,Precision> rot = se3.get_rotation().ln();
00432 const Precision theta = sqrt(rot*rot);
00433
00434 Precision shtot = 0.5;
00435 if(theta > 0.00001) {
00436 shtot = sin(theta/2)/theta;
00437 }
00438
00439
00440 const SO3<Precision> halfrotator = SO3<Precision>::exp(rot * -0.5);
00441 Vector<3, Precision> rottrans = halfrotator * se3.get_translation();
00442
00443 if(theta > 0.001){
00444 rottrans -= TooN::operator*(rot, ((se3.get_translation() * rot) * (1-2*shtot) / (rot*rot)));
00445 } else {
00446 rottrans -= TooN::operator*(rot, ((se3.get_translation() * rot)/24));
00447 }
00448
00449 rottrans /= (2 * shtot);
00450
00451 Vector<6, Precision> result;
00452 result.template slice<0,3>()=rottrans;
00453 result.template slice<3,3>()=rot;
00454 return result;
00455 }
00456
00457 template <typename Precision>
00458 inline SE3<Precision> operator*(const SO3<Precision>& lhs, const SE3<Precision>& rhs){
00459 return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
00460 }
00461
00462 #if 0 // should be moved to another header file
00463
00464 template <class A> inline
00465 Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return pose.get_rotation()*x + pose.get_translation(); }
00466
00467 template <class A> inline
00468 Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A>& uvq)
00469 {
00470 const Matrix<3>& R = pose.get_rotation().get_matrix();
00471 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
00472 const double inv_z = 1.0/ DqT[2];
00473 return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
00474 }
00475
00476 template <class A1, class A2> inline
00477 Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
00478 {
00479 J_x = pose.get_rotation().get_matrix();
00480 return pose * x;
00481 }
00482
00483
00484 template <class A1, class A2> inline
00485 Vector<3> inverse_depth_point_jacobian(const SE3& pose, const double q, const FixedVector<3,A1>& DqT, const double inv_z, FixedMatrix<3,3,A2>& J_uvq)
00486 {
00487 const Matrix<3>& R = pose.get_rotation().get_matrix();
00488 const Vector<3>& T = pose.get_translation();
00489 const double u1 = DqT[0] * inv_z;
00490 J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
00491 J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
00492 J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
00493
00494 const double v1 = DqT[1] * inv_z;
00495 J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
00496 J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
00497 J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
00498
00499 const double q1 = q * inv_z;
00500 J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
00501 J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
00502 J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
00503
00504 return makeVector(u1, v1, q1);
00505 }
00506
00507
00508 template <class A1, class A2> inline
00509 Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
00510 {
00511 const Matrix<3>& R = pose.get_rotation().get_matrix();
00512 const Vector<3>& T = pose.get_translation();
00513 const double q = uvq[2];
00514 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00515 const double inv_z = 1.0 / DqT[2];
00516
00517 return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00518 }
00519
00520 template <class A1, class A2, class A3> inline
00521 Vector<3> transform(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
00522 {
00523 J_x = pose.get_rotation().get_matrix();
00524 Identity(J_pose.template slice<0,0,3,3>());
00525 const Vector<3> cx = pose * x;
00526 J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
00527 J_pose[1][3] = -(J_pose[0][4] = cx[2]);
00528 J_pose[0][5] = -(J_pose[2][3] = cx[1]);
00529 J_pose[2][4] = -(J_pose[1][5] = cx[0]);
00530 return cx;
00531 }
00532
00533 template <class A1, class A2, class A3> inline
00534 Vector<3> transform_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq, FixedMatrix<3,6,A3>& J_pose)
00535 {
00536 const Matrix<3>& R = pose.get_rotation().get_matrix();
00537 const Vector<3>& T = pose.get_translation();
00538 const double q = uvq[2];
00539 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00540 const double inv_z = 1.0 / DqT[2];
00541
00542 const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
00543 const double q1 = uvq1[2];
00544
00545 J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
00546 J_pose[0][0] = J_pose[1][1] = q1;
00547 J_pose[0][2] = -uvq1[0] * q1;
00548 J_pose[1][2] = -uvq1[1] * q1;
00549 J_pose[2][2] = -q1 * q1;
00550
00551 J_pose[0][3] = -uvq1[1]*uvq1[0];
00552 J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
00553 J_pose[0][5] = -uvq1[1];
00554
00555 J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
00556 J_pose[1][4] = uvq1[0] * uvq1[1];
00557 J_pose[1][5] = uvq1[0];
00558
00559 J_pose[2][3] = -uvq1[1]*q1;
00560 J_pose[2][4] = uvq1[0]*q1;
00561 J_pose[2][5] = 0;
00562
00563 return uvq1;
00564 }
00565
00566 template <class A1, class A2, class A3> inline
00567 Vector<2> project_transformed_point(const SE3& pose, const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose)
00568 {
00569 const double z_inv = 1.0/in_frame[2];
00570 const double x_z_inv = in_frame[0]*z_inv;
00571 const double y_z_inv = in_frame[1]*z_inv;
00572 const double cross = x_z_inv * y_z_inv;
00573 J_pose[0][0] = J_pose[1][1] = z_inv;
00574 J_pose[0][1] = J_pose[1][0] = 0;
00575 J_pose[0][2] = -x_z_inv * z_inv;
00576 J_pose[1][2] = -y_z_inv * z_inv;
00577 J_pose[0][3] = -cross;
00578 J_pose[0][4] = 1 + x_z_inv*x_z_inv;
00579 J_pose[0][5] = -y_z_inv;
00580 J_pose[1][3] = -1 - y_z_inv*y_z_inv;
00581 J_pose[1][4] = cross;
00582 J_pose[1][5] = x_z_inv;
00583
00584 const TooN::Matrix<3>& R = pose.get_rotation().get_matrix();
00585 J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
00586 J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
00587 J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
00588 J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
00589 J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
00590 J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
00591
00592 return makeVector(x_z_inv, y_z_inv);
00593 }
00594
00595
00596 template <class A1> inline
00597 Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x)
00598 {
00599 return project(transform(pose,x));
00600 }
00601
00602 template <class A1> inline
00603 Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq)
00604 {
00605 const Matrix<3>& R = pose.get_rotation().get_matrix();
00606 const Vector<3>& T = pose.get_translation();
00607 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * T;
00608 return project(DqT);
00609 }
00610
00611 template <class A1, class A2, class A3> inline
00612 Vector<2> transform_and_project(const SE3& pose, const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,6,A3>& J_pose)
00613 {
00614 return project_transformed_point(pose, transform(pose,x), J_x, J_pose);
00615 }
00616
00617 template <class A1, class A2, class A3> inline
00618 Vector<2> transform_and_project_inverse_depth(const SE3& pose, const FixedVector<3,A1>& uvq, FixedMatrix<2,3,A2>& J_uvq, FixedMatrix<2,6,A3>& J_pose)
00619 {
00620 const Matrix<3>& R = pose.get_rotation().get_matrix();
00621 const Vector<3>& T = pose.get_translation();
00622 const double q = uvq[2];
00623 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
00624 const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, J_pose);
00625 J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * pose.get_translation();
00626 J_pose.template slice<0,0,2,3>() *= q;
00627 return uv;
00628 }
00629
00630 #endif
00631
00632 }
00633
00634 #endif