30 #ifndef TOON_INCLUDE_SE3_H
31 #define TOON_INCLUDE_SE3_H
49 template <
typename Precision =
double>
55 template <
int S,
typename P,
typename A>
57 template <
int S,
typename P,
typename A>
60 template <
class IP,
int S,
typename P,
typename A>
76 template <
int S,
typename P,
typename A>
118 result[(i+1)%3][(i+2)%3] = -1;
119 result[(i+2)%3][(i+1)%3] = 1;
124 template<
typename Base>
132 result[(i+1)%3] = - pos[(i+2)%3];
133 result[(i+2)%3] = pos[(i+1)%3];
142 template<
int S,
typename P2,
typename Accessor>
147 template<
int S,
typename P2,
typename Accessor>
151 template <
int R,
int C,
typename P2,
typename Accessor>
155 template <
int R,
int C,
typename P2,
typename Accessor>
166 template<
typename Precision>
167 template<
int S,
typename P2,
typename Accessor>
171 result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
172 result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
173 result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
180 template<
typename Precision>
181 template<
int S,
typename P2,
typename Accessor>
185 result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
186 result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
187 result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
191 template<
typename Precision>
192 template<
int R,
int C,
typename P2,
typename Accessor>
198 for(
int i=0; i<6; i++){
199 result.T()[i] = adjoint(M.T()[i]);
201 for(
int i=0; i<6; i++){
202 result[i] = adjoint(result[i]);
207 template<
typename Precision>
208 template<
int R,
int C,
typename P2,
typename Accessor>
214 for(
int i=0; i<6; i++){
215 result.T()[i] = trinvadjoint(M.T()[i]);
217 for(
int i=0; i<6; i++){
218 result[i] = trinvadjoint(result[i]);
225 template <
typename Precision>
226 inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
227 std::streamsize fw = os.width();
228 for(
int i=0; i<3; i++){
230 os << rhs.get_rotation().get_matrix()[i];
232 os << rhs.get_translation()[i] <<
'\n';
240 template <
typename Precision>
242 for(
int i=0; i<3; i++){
255 template<
int S,
typename PV,
typename A,
typename P>
259 template<
int S,
typename PV,
typename A,
typename P>
266 template <
int S0,
typename P0,
typename A0>
269 res.template slice<0,3>()=lhs.
get_rotation() * rhs.template slice<0,3>();
278 template<
int S,
typename PV,
typename A,
typename P>
inline
285 template <
typename PV,
typename A,
typename P>
inline
296 template<
int S,
typename PV,
typename A,
typename P>
300 template<
int S,
typename PV,
typename A,
typename P>
307 template <
int S0,
typename P0,
typename A0>
310 res.template slice<0,3>()=lhs.template slice<0,3>() * rhs.
get_rotation();
319 template<
int S,
typename PV,
typename A,
typename P>
inline
330 template <
int R,
int C,
typename PM,
typename A,
typename P>
334 template<
int R,
int Cols,
typename PM,
typename A,
typename P>
335 struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
341 template <
int R0,
int C0,
typename P0,
typename A0>
344 for(
int i=0; i<rhs.num_cols(); ++i)
345 res.T()[i] = lhs * rhs.T()[i];
353 template <
int R,
int Cols,
typename PM,
typename A,
typename P>
inline
364 template <
int Rows,
int C,
typename PM,
typename A,
typename P>
368 template<
int Rows,
int C,
typename PM,
typename A,
typename P>
369 struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
375 template <
int R0,
int C0,
typename P0,
typename A0>
378 for(
int i=0; i<lhs.num_rows(); ++i)
379 res[i] = lhs[i] * rhs;
387 template <
int Rows,
int C,
typename PM,
typename A,
typename P>
inline
392 template <
typename Precision>
393 template <
int S,
typename P,
typename VA>
396 static const Precision one_6th = 1.0/6.0;
397 static const Precision one_20th = 1.0/20.0;
402 const Precision theta_sq = w*w;
403 const Precision theta = sqrt(theta_sq);
407 if (theta_sq < 1e-8) {
408 A = 1.0 - one_6th * theta_sq;
413 if (theta_sq < 1e-6) {
414 C = one_6th*(1.0 - one_20th * theta_sq);
415 A = 1.0 - theta_sq * C;
416 B = 0.5 - 0.25 * one_6th * theta_sq;
418 const Precision inv_theta = 1.0/theta;
419 A = sin(theta) * inv_theta;
420 B = (1 - cos(theta)) * (inv_theta * inv_theta);
421 C = (1 - A) * (inv_theta * inv_theta);
425 rodrigues_so3_exp(w, A, B, result.
get_rotation().my_matrix);
429 template <
typename Precision>
432 const Precision theta = sqrt(rot*rot);
434 Precision shtot = 0.5;
435 if(theta > 0.00001) {
436 shtot = sin(theta/2)/theta;
449 rottrans /= (2 * shtot);
452 result.template slice<0,3>()=rottrans;
453 result.template slice<3,3>()=rot;
457 template <
typename Precision>
462 #if 0 // should be moved to another header file
464 template <
class A>
inline
465 Vector<3>
transform(
const SE3& pose,
const FixedVector<3,A>& x) {
return pose.get_rotation()*x + pose.get_translation(); }
467 template <
class A>
inline
468 Vector<3> transform_inverse_depth(
const SE3& pose,
const FixedVector<3,A>& uvq)
470 const Matrix<3>& R = pose.get_rotation().get_matrix();
471 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * pose.get_translation();
472 const double inv_z = 1.0/ DqT[2];
473 return makeVector(DqT[0]*inv_z, DqT[1]*inv_z, uvq[2]*inv_z);
476 template <
class A1,
class A2>
inline
477 Vector<3>
transform(
const SE3& pose,
const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
479 J_x = pose.get_rotation().get_matrix();
484 template <
class A1,
class A2>
inline
485 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)
487 const Matrix<3>& R = pose.get_rotation().get_matrix();
488 const Vector<3>& T = pose.get_translation();
489 const double u1 = DqT[0] * inv_z;
490 J_uvq[0][0] = inv_z * (R[0][0] - u1*R[2][0]);
491 J_uvq[0][1] = inv_z * (R[0][1] - u1*R[2][1]);
492 J_uvq[0][2] = inv_z * (T[0] - u1 * T[2]);
494 const double v1 = DqT[1] * inv_z;
495 J_uvq[1][0] = inv_z * (R[1][0] - v1*R[2][0]);
496 J_uvq[1][1] = inv_z * (R[1][1] - v1*R[2][1]);
497 J_uvq[1][2] = inv_z * (T[1] - v1 * T[2]);
499 const double q1 = q * inv_z;
500 J_uvq[2][0] = inv_z * (-q1 * R[2][0]);
501 J_uvq[2][1] = inv_z * (-q1 * R[2][1]);
502 J_uvq[2][2] = inv_z * (1.0 - q1 * T[2]);
508 template <
class A1,
class A2>
inline
509 Vector<3> transform_inverse_depth(
const SE3& pose,
const FixedVector<3,A1>& uvq, FixedMatrix<3,3,A2>& J_uvq)
511 const Matrix<3>& R = pose.get_rotation().get_matrix();
512 const Vector<3>& T = pose.get_translation();
513 const double q = uvq[2];
514 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
515 const double inv_z = 1.0 / DqT[2];
517 return inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
520 template <
class A1,
class A2,
class A3>
inline
521 Vector<3>
transform(
const SE3& pose,
const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,6,A3>& J_pose)
523 J_x = pose.get_rotation().get_matrix();
524 Identity(J_pose.template slice<0,0,3,3>());
525 const Vector<3> cx = pose * x;
526 J_pose[0][3] = J_pose[1][4] = J_pose[2][5] = 0;
527 J_pose[1][3] = -(J_pose[0][4] = cx[2]);
528 J_pose[0][5] = -(J_pose[2][3] = cx[1]);
529 J_pose[2][4] = -(J_pose[1][5] = cx[0]);
533 template <
class A1,
class A2,
class A3>
inline
534 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)
536 const Matrix<3>& R = pose.get_rotation().get_matrix();
537 const Vector<3>& T = pose.get_translation();
538 const double q = uvq[2];
539 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
540 const double inv_z = 1.0 / DqT[2];
542 const Vector<3> uvq1 = inverse_depth_point_jacobian(pose, q, DqT, inv_z, J_uvq);
543 const double q1 = uvq1[2];
545 J_pose[0][1] = J_pose[1][0] = J_pose[2][0] = J_pose[2][1] = 0;
546 J_pose[0][0] = J_pose[1][1] = q1;
547 J_pose[0][2] = -uvq1[0] * q1;
548 J_pose[1][2] = -uvq1[1] * q1;
549 J_pose[2][2] = -q1 * q1;
551 J_pose[0][3] = -uvq1[1]*uvq1[0];
552 J_pose[0][4] = 1 + uvq1[0]*uvq1[0];
553 J_pose[0][5] = -uvq1[1];
555 J_pose[1][3] = -1 - uvq1[1]*uvq1[1];
556 J_pose[1][4] = uvq1[0] * uvq1[1];
557 J_pose[1][5] = uvq1[0];
559 J_pose[2][3] = -uvq1[1]*q1;
560 J_pose[2][4] = uvq1[0]*q1;
566 template <
class A1,
class A2,
class A3>
inline
567 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)
569 const double z_inv = 1.0/in_frame[2];
570 const double x_z_inv = in_frame[0]*z_inv;
571 const double y_z_inv = in_frame[1]*z_inv;
572 const double cross = x_z_inv * y_z_inv;
573 J_pose[0][0] = J_pose[1][1] = z_inv;
574 J_pose[0][1] = J_pose[1][0] = 0;
575 J_pose[0][2] = -x_z_inv * z_inv;
576 J_pose[1][2] = -y_z_inv * z_inv;
577 J_pose[0][3] = -cross;
578 J_pose[0][4] = 1 + x_z_inv*x_z_inv;
579 J_pose[0][5] = -y_z_inv;
580 J_pose[1][3] = -1 - y_z_inv*y_z_inv;
581 J_pose[1][4] = cross;
582 J_pose[1][5] = x_z_inv;
585 J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
586 J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
587 J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
588 J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
589 J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
590 J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
596 template <
class A1>
inline
597 Vector<2> transform_and_project(
const SE3& pose,
const FixedVector<3,A1>& x)
602 template <
class A1>
inline
603 Vector<2> transform_and_project_inverse_depth(
const SE3& pose,
const FixedVector<3,A1>& uvq)
605 const Matrix<3>& R = pose.get_rotation().get_matrix();
606 const Vector<3>& T = pose.get_translation();
607 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + uvq[2] * T;
611 template <
class A1,
class A2,
class A3>
inline
612 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)
614 return project_transformed_point(pose,
transform(pose,x), J_x, J_pose);
617 template <
class A1,
class A2,
class A3>
inline
618 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)
620 const Matrix<3>& R = pose.get_rotation().get_matrix();
621 const Vector<3>& T = pose.get_translation();
622 const double q = uvq[2];
623 const Vector<3> DqT = R.template slice<0,0,3,2>() * uvq.template slice<0,2>() + R.T()[2] + q * T;
624 const Vector<2> uv = project_transformed_point(pose, DqT, J_uvq, J_pose);
625 J_uvq.T()[2] = J_pose.template slice<0,0,2,3>() * pose.get_translation();
626 J_pose.template slice<0,0,2,3>() *= q;
SE3(const Vector< S, P, A > &v)
static SO3 exp(const Vector< S, VP, A > &vect)
static SE3 exp(const Vector< S, P, A > &vect)
SO3< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO3.
static Operator< Internal::Identity< Internal::One > > Identity
Vector< 3, typename Internal::MultiplyType< P, PV >::type > operator*(const SE3< P > &lhs, const Vector< 3, PV, A > &rhs)
SE3< typename Internal::MultiplyType< Precision, P >::type > operator*(const SE3< P > &rhs) const
const Vector< 3, Precision > & get_translation() const
Everything lives inside this namespace.
const Vector< S, PV, A > & rhs
void eval(Matrix< R0, C0, P0, A0 > &res) const
static void test(int s1, int s2)
SE3(const SO3< Precision > &R, const Vector< S, P, A > &T)
Vector< 6, Precision > adjoint(const Vector< S, P2, Accessor > &vect) const
Matrix< 4, Cols, typename Internal::MultiplyType< P, PM >::type > operator*(const SE3< P > &lhs, const Matrix< R, Cols, PM, A > &rhs)
Vector< 6, Precision > ln() const
const Matrix< R, Cols, PM, A > & rhs
Operator(const SE3< P > &l, const Vector< S, PV, A > &r)
std::istream & operator>>(std::istream &is, SE3< Precision > &rhs)
int transform(const BasicImage< S > &in, BasicImage< T > &out, const TooN::Matrix< 2 > &M, const TooN::Vector< 2 > &inOrig, const TooN::Vector< 2 > &outOrig, const T defaultValue=T())
static Vector< 4, Precision > generator_field(int i, const Vector< 4, Precision, Base > &pos)
Returns the i-th generator times pos.
void eval(Matrix< R0, C0, P0, A0 > &res) const
SE3 & left_multiply_by(const SE3 &left)
Operator(const Vector< S, PV, A > &l, const SE3< P > &r)
Vector< 3, Precision > & get_translation()
Returns the translation part of the transformation as a Vector.
const Matrix< Rows, C, PM, A > & lhs
Vector< 1 > makeVector(double x1)
Vector< 4, typename Internal::MultiplyType< P, PV >::type > operator*(const Vector< S, PV, A > &lhs, const SE3< P > &rhs)
SE3()
Default constructor. Initialises the the rotation to zero (the identity) and the translation to zero...
Vector< 3, Precision > my_translation
Matrix< Rows, 4, typename Internal::MultiplyType< PM, P >::type > operator*(const Matrix< Rows, C, PM, A > &lhs, const SE3< P > &rhs)
const SO3< Precision > & get_rotation() const
Operator(const Matrix< Rows, C, PM, A > &l, const SE3< P > &r)
void eval(Vector< S0, P0, A0 > &res) const
Operator(const SE3< P > &l, const Matrix< R, Cols, PM, A > &r)
SO3< Precision > my_rotation
SE3(const Operator< Internal::Identity< IP > > &, const Vector< S, P, A > &T)
static Matrix< 4, 4, Precision > generator(int i)
const Vector< S, PV, A > & lhs
Vector< 6, Precision > trinvadjoint(const Vector< S, P2, Accessor > &vect) const
Vector< Internal::Sizer< S1, S2 >::size, typename Internal::MultiplyType< P1, P2 >::type > operator*(const DiagonalMatrix< S1, P1, B1 > &d, const Vector< S2, P2, B2 > &v)
static Operator< Internal::Zero > Zeros
void eval(Vector< S0, P0, A0 > &res) const
Vector<(Size==Dynamic?Dynamic:Size-1), Precision > project(const Vector< Size, Precision, Base > &v)
SE3 & operator*=(const SE3 &rhs)
Vector< 4, typename Internal::MultiplyType< P, PV >::type > operator*(const SE3< P > &lhs, const Vector< S, PV, A > &rhs)