30 #ifndef TOON_INCLUDE_SO3_H
31 #define TOON_INCLUDE_SO3_H
39 template <
typename Precision>
class SO3;
40 template <
typename Precision>
class SE3;
52 template <
typename Precision =
double>
55 friend std::istream&
operator>> <Precision> (std::istream& is,
SO3<Precision> & rhs);
56 friend std::istream&
operator>> <Precision> (std::istream& is,
SE3<Precision> & rhs);
57 friend class SE3<Precision>;
63 template <
int S,
typename P,
typename A>
67 template <
int R,
int C,
typename P,
typename A>
75 template <
int S1,
int S2,
typename P1,
typename P2,
typename A1,
typename A2>
91 R1.T()[2] = n ^ R1.T()[0];
100 template <
int R,
int C,
typename P,
typename A>
112 my_matrix[2] -= my_matrix[0] * (my_matrix[0]*my_matrix[2]);
113 my_matrix[2] -= my_matrix[1] * (my_matrix[1]*my_matrix[2]);
114 my_matrix[2] =
unit(my_matrix[2]);
116 assert( (my_matrix[0] ^ my_matrix[1]) * my_matrix[2] > 0 );
151 result[(i+1)%3][(i+2)%3] = -1;
152 result[(i+2)%3][(i+1)%3] = 1;
157 template<
typename Base>
162 result[(i+1)%3] = - pos[(i+2)%3];
163 result[(i+2)%3] = pos[(i+1)%3];
171 template <
int S,
typename A>
178 template <
typename PA,
typename PB>
190 template <
typename Precision>
191 inline std::ostream& operator<< (std::ostream& os, const SO3<Precision>& rhs){
192 return os << rhs.get_matrix();
197 template <
typename Precision>
214 template <
typename Precision,
int S,
typename VP,
typename VA,
typename MA>
218 const Precision wx2 = (Precision)w[0]*w[0];
219 const Precision wy2 = (Precision)w[1]*w[1];
220 const Precision wz2 = (Precision)w[2]*w[2];
222 R[0][0] = 1.0 - B*(wy2 + wz2);
223 R[1][1] = 1.0 - B*(wx2 + wz2);
224 R[2][2] = 1.0 - B*(wx2 + wy2);
227 const Precision a = A*w[2];
228 const Precision b = B*(w[0]*w[1]);
233 const Precision a = A*w[1];
234 const Precision b = B*(w[0]*w[2]);
239 const Precision a = A*w[0];
240 const Precision b = B*(w[1]*w[2]);
248 template <
typename Precision>
249 template<
int S,
typename VP,
typename VA>
256 static const Precision one_6th = 1.0/6.0;
257 static const Precision one_20th = 1.0/20.0;
261 const Precision theta_sq = w*w;
262 const Precision theta = sqrt(theta_sq);
266 if (theta_sq < 1e-8) {
267 A = 1.0 - one_6th * theta_sq;
270 if (theta_sq < 1e-6) {
271 B = 0.5 - 0.25 * one_6th * theta_sq;
272 A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
274 const Precision inv_theta = 1.0/theta;
275 A = sin(theta) * inv_theta;
276 B = (1 - cos(theta)) * (inv_theta * inv_theta);
279 rodrigues_so3_exp(w, A, B, result.
my_matrix);
283 template <
typename Precision>
287 const Precision cos_angle = (my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2] - 1.0) * 0.5;
288 result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
289 result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
290 result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
292 Precision sin_angle_abs = sqrt(result*result);
293 if (cos_angle > M_SQRT1_2) {
294 if(sin_angle_abs > 0){
295 result *= asin(sin_angle_abs) / sin_angle_abs;
297 }
else if( cos_angle > -M_SQRT1_2) {
298 const Precision angle = acos(cos_angle);
299 result *= angle / sin_angle_abs;
302 const Precision angle = M_PI - asin(sin_angle_abs);
303 const Precision d0 = my_matrix[0][0] - cos_angle,
304 d1 = my_matrix[1][1] - cos_angle,
305 d2 = my_matrix[2][2] - cos_angle;
307 if(d0*d0 > d1*d1 && d0*d0 > d2*d2){
309 r2[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
310 r2[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
311 }
else if(d1*d1 > d2*d2) {
312 r2[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
314 r2[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
316 r2[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
317 r2[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
331 template<
int S,
typename P,
typename PV,
typename A>
inline
338 template<
int S,
typename P,
typename PV,
typename A>
inline
345 template<
int R,
int C,
typename P,
typename PM,
typename A>
inline
352 template<
int R,
int C,
typename P,
typename PM,
typename A>
inline
357 #if 0 // will be moved to another header file ?
359 template <
class A>
inline
362 template <
class A1,
class A2>
inline
363 Vector<3>
transform(
const SO3& pose,
const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x)
365 J_x = pose.get_matrix();
369 template <
class A1,
class A2,
class A3>
inline
370 Vector<3>
transform(
const SO3& pose,
const FixedVector<3,A1>& x, FixedMatrix<3,3,A2>& J_x, FixedMatrix<3,3,A3>& J_pose)
372 J_x = pose.get_matrix();
373 const Vector<3> cx = pose * x;
374 J_pose[0][0] = J_pose[1][1] = J_pose[2][2] = 0;
375 J_pose[1][0] = -(J_pose[0][1] = cx[2]);
376 J_pose[0][2] = -(J_pose[2][0] = cx[1]);
377 J_pose[2][1] = -(J_pose[1][2] = cx[0]);
381 template <
class A1,
class A2,
class A3>
inline
382 Vector<2> project_transformed_point(
const SO3& pose,
const FixedVector<3,A1>& in_frame, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,3,A3>& J_pose)
384 const double z_inv = 1.0/in_frame[2];
385 const double x_z_inv = in_frame[0]*z_inv;
386 const double y_z_inv = in_frame[1]*z_inv;
387 const double cross = x_z_inv * y_z_inv;
388 J_pose[0][0] = -cross;
389 J_pose[0][1] = 1 + x_z_inv*x_z_inv;
390 J_pose[0][2] = -y_z_inv;
391 J_pose[1][0] = -1 - y_z_inv*y_z_inv;
392 J_pose[1][1] = cross;
393 J_pose[1][2] = x_z_inv;
396 J_x[0][0] = z_inv*(R[0][0] - x_z_inv * R[2][0]);
397 J_x[0][1] = z_inv*(R[0][1] - x_z_inv * R[2][1]);
398 J_x[0][2] = z_inv*(R[0][2] - x_z_inv * R[2][2]);
399 J_x[1][0] = z_inv*(R[1][0] - y_z_inv * R[2][0]);
400 J_x[1][1] = z_inv*(R[1][1] - y_z_inv * R[2][1]);
401 J_x[1][2] = z_inv*(R[1][2] - y_z_inv * R[2][2]);
407 template <
class A1>
inline
408 Vector<2> transform_and_project(
const SO3& pose,
const FixedVector<3,A1>& x)
413 template <
class A1,
class A2,
class A3>
inline
414 Vector<2> transform_and_project(
const SO3& pose,
const FixedVector<3,A1>& x, FixedMatrix<2,3,A2>& J_x, FixedMatrix<2,3,A3>& J_pose)
416 return project_transformed_point(pose,
transform(pose,x), J_x, J_pose);
SO3 inverse() const
Returns the inverse of this matrix (=the transpose, so this is a fast operation)
static SO3 exp(const Vector< S, VP, A > &vect)
static Operator< Internal::Identity< Internal::One > > Identity
Everything lives inside this namespace.
SO3 & operator*=(const SO3 &rhs)
Right-multiply by another rotation matrix.
static void test(int s1, int s2)
Vector< 3, typename Internal::MultiplyType< P, PV >::type > operator*(const SO3< P > &lhs, const Vector< S, PV, A > &rhs)
SO3()
Default constructor. Initialises the matrix to the identity (no rotation)
Vector< 3, Precision > ln() const
Matrix< R, 3, typename Internal::MultiplyType< PM, P >::type > operator*(const Matrix< R, C, PM, A > &lhs, const SO3< P > &rhs)
SO3(const Vector< S1, P1, A1 > &a, const Vector< S2, P2, A2 > &b)
SO3 & operator=(const Matrix< R, C, P, A > &rhs)
Vector< 3, Precision > adjoint(const Vector< S, Precision, A > &vect) const
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())
SO3(const SO3 &so3, const Invert &)
void coerce()
Modifies the matrix to make sure it is a valid rotation matrix.
Vector< 1 > makeVector(double x1)
Precision norm_sq(const Vector< Size, Precision, Base > &v)
std::istream & operator>>(std::istream &is, SO3< Precision > &rhs)
void rodrigues_so3_exp(const Vector< S, VP, VA > &w, const Precision A, const Precision B, Matrix< 3, 3, Precision, MA > &R)
Vector< 3, typename Internal::MultiplyType< PV, P >::type > operator*(const Vector< S, PV, A > &lhs, const SO3< P > &rhs)
Vector< Size, Precision > unit(const Vector< Size, Precision, Base > &v)
SO3(const SO3< PA > &a, const SO3< PB > &b)
SO3(const Matrix< R, C, P, A > &rhs)
Construct from a rotation matrix.
std::istream & operator>>(std::istream &is, Vector< Size, Precision, Base > &v)
static Matrix< 3, 3, Precision > generator(int i)
SO3< typename Internal::MultiplyType< Precision, P >::type > operator*(const SO3< P > &rhs) const
Right-multiply by another rotation matrix.
Matrix< 3, C, typename Internal::MultiplyType< P, PM >::type > operator*(const SO3< P > &lhs, const Matrix< R, C, PM, A > &rhs)
static Vector< 3, Precision > generator_field(int i, const Vector< 3, Precision, Base > &pos)
Returns the i-th generator times pos.
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)
Matrix< 3, 3, Precision > my_matrix
const Matrix< 3, 3, Precision > & get_matrix() const
Returns the SO3 as a Matrix<3>
static Operator< Internal::Zero > Zeros
Vector<(Size==Dynamic?Dynamic:Size-1), Precision > project(const Vector< Size, Precision, Base > &v)
SO3(const Vector< S, P, A > &v)
Construct from the axis of rotation (and angle given by the magnitude).