[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN se2.h se3.h so2.h so3.h test/SXX_test.cc
|
From: |
Gerhard Reitmayr |
|
Subject: |
[Toon-members] TooN se2.h se3.h so2.h so3.h test/SXX_test.cc |
|
Date: |
Thu, 02 Apr 2009 13:36:04 +0000 |
CVSROOT: /cvsroot/toon
Module name: TooN
Changes by: Gerhard Reitmayr <gerhard> 09/04/02 13:36:04
Modified files:
. : se2.h se3.h so2.h so3.h
test : SXX_test.cc
Log message:
ported so2, se2, so3, se3 to TooN2
the transform_XXXX functions are currently just disabled, they will be
cleaned up and moved in the next step.
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/se2.h?cvsroot=toon&r1=1.1&r2=1.2
http://cvs.savannah.gnu.org/viewcvs/TooN/se3.h?cvsroot=toon&r1=1.17&r2=1.18
http://cvs.savannah.gnu.org/viewcvs/TooN/so2.h?cvsroot=toon&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.24&r2=1.25
http://cvs.savannah.gnu.org/viewcvs/TooN/test/SXX_test.cc?cvsroot=toon&r1=1.5&r2=1.6
Patches:
Index: se2.h
===================================================================
RCS file: /cvsroot/toon/TooN/se2.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- se2.h 22 Aug 2007 14:17:15 -0000 1.1
+++ se2.h 2 Apr 2009 13:36:03 -0000 1.2
@@ -29,83 +29,86 @@
namespace TooN {
#endif
+template <typename Precision = double>
class SE2 {
- friend SE2 operator*(const SO2& lhs, const SE2& rhs);
- friend std::istream& operator>> (std::istream& is, SE2& rhs);
+public:
+ SE2() : my_translation(Zero) {}
+ template <class A> SE2(const SO2<Precision>& R, const
Vector<2,Precision,A>& T) : my_rotation(R), my_translation(T) {}
+ template <int S, class P, class A> SE2(const Vector<S, P, A> & v) {
*this = exp(v); }
+
+ SO2<Precision> & get_rotation(){return my_rotation;}
+ const SO2<Precision> & get_rotation() const {return my_rotation;}
+ Vector<2, Precision> & get_translation() {return my_translation;}
+ const Vector<2, Precision> & get_translation() const {return
my_translation;}
+
+ template <int S, typename P, typename A>
+ static inline SE2 exp(const Vector<S,P, A>& vect);
+ static inline Vector<3, Precision> ln(const SE2& se2);
+ Vector<3, Precision> ln() const { return SE2::ln(*this); }
- public:
- inline SE2();
- template <class A> inline SE2(const SO2& R, const FixedVector<2,A>& T) :
my_rotation(R), my_translation(T) {}
-
-
- inline SO2& get_rotation(){return my_rotation;}
- inline const SO2& get_rotation() const {return my_rotation;}
- inline Vector<2>& get_translation() {return my_translation;}
- inline const Vector<2>& get_translation() const {return my_translation;}
-
- static inline SE2 exp(const Vector<3>& vect);
- static inline Vector<3> ln(const SE2& se2);
- inline Vector<3> ln() const { return SE2::ln(*this); }
-
- inline SE2 inverse() const;
+ SE2 inverse() const {
+ const SO2<Precision> & rinv = my_rotation.inverse();
+ return SE2(rinv, -(rinv*my_translation));
+ };
- inline SE2& operator *=(const SE2& rhs);
inline SE2 operator *(const SE2& rhs) const { return
SE2(my_rotation*rhs.my_rotation, my_translation +
my_rotation*rhs.my_translation); }
- inline SE2& left_multiply_by(const SE2& left);
-
- static inline Vector<3> generator_field(int i, Vector<3> pos);
+ inline SE2& operator *=(const SE2& rhs) {
+ *this = *this * rhs;
+ return *this;
+ }
+ static inline Matrix<3,3, Precision> generator(int i) {
+ Matrix<3,3,Precision> result(Zero);
+ if(i < 2){
+ result[i][2] = 1;
+ return result;
+ }
+ result[0][1] = -1;
+ result[1][0] = 1;
+ return result;
+ }
- template<class Accessor>
- inline void adjoint(FixedVector<3,Accessor>& vect)const;
+ /// transfers a vector in the Lie algebra, from one coord frame to
another
+ /// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
+ template<typename Accessor>
+ inline Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor>
& vect) const {
+ Vector<3, Precision> result;
+ result[2] = vect[2];
+ result.template slice<0,2>() = my_rotation * vect.template
slice<0,2>();
+ result[0] += vect[2] * my_translation[1];
+ result[1] -= vect[2] * my_translation[0];
+ return result;
+ }
- template <class Accessor>
- inline void adjoint(FixedMatrix<3,3,Accessor>& M)const;
+ template <typename Accessor>
+ inline Matrix<3,3,Precision> adjoint(const
Matrix<3,3,Precision,Accessor>& M) const {
+ Matrix<3,3,Precision> result;
+ for(int i=0; i<3; ++i)
+ result.T()[i] = adjoint(M.T()[i]);
+ for(int i=0; i<3; ++i)
+ result[i] = adjoint(result[i]);
+ return result;
+ }
private:
- SO2 my_rotation;
- Vector<2> my_translation;
+ SO2<Precision> my_rotation;
+ Vector<2, Precision> my_translation;
};
-
-// left multiply an SE2 by an SO2
-inline SE2 operator*(const SO2& lhs, const SE2& rhs);
-
-
-// transfers a vector in the Lie algebra
-// from one coord frame to another
-// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
-template<class Accessor>
- inline void SE2::adjoint(FixedVector<3,Accessor>& vect)const{
- vect.template slice<0,2>() = my_rotation * vect.template slice<0,2>();
- vect[0] += vect[2] * my_translation[1];
- vect[1] -= vect[2] * my_translation[0];
- }
-
-template <class Accessor>
-inline void SE2::adjoint(FixedMatrix<3,3,Accessor>& M)const{
- for(int i=0; i<3; i++){
- adjoint(M.T()[i]);
- }
- for(int i=0; i<3; i++){
- adjoint(M[i]);
- }
-}
-
-
// operator ostream& <<
-inline std::ostream& operator <<(std::ostream& os, const SE2& rhs){
- for(int i=0; i<2; i++){
+template <class Precision>
+inline std::ostream& operator<<(std::ostream& os, const SE2<Precision> & rhs){
+ for(int i=0; i<2; i++)
os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] <<
std::endl;
- }
return os;
}
// operator istream& >>
-inline std::istream& operator>>(std::istream& is, SE2& rhs){
- for(int i=0; i<2; i++){
- is >> rhs.get_rotation().my_matrix[i] >> rhs.get_translation()[i];
- }
+template <class Precision>
+inline std::istream& operator>>(std::istream& is, SE2<Precision>& rhs){
+ for(int i=0; i<2; i++)
+ is >> rhs.get_rotation().my_matrix[i].ref() >>
rhs.get_translation()[i];
+ SO2<Precision>::coerce(rhs.get_rotation().my_matrix);
return is;
}
@@ -115,109 +118,134 @@
// SE2 * Vector //
//////////////////
-template<class VectorType>
-struct SE2VMult {
- inline static void eval(Vector<3>& ret, const SE2& lhs, const VectorType&
rhs){
- ret.template slice<0,2>()=lhs.get_rotation()*rhs.template slice<0,2>();
- ret.template slice<0,2>()+=lhs.get_translation() * rhs[2];
- ret[2] = rhs[2];
+namespace Internal {
+template<int S, typename P, typename PV, typename A>
+struct SE2VMult;
+}
+
+template<int S, typename P, typename PV, typename A>
+struct Operator<Internal::SE2VMult<S,P,PV,A> > {
+ const SE2<P> & lhs;
+ const Vector<S,PV,A> & rhs;
+
+ Operator(const SE2<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r)
{}
+
+ template <int S0, typename P0, typename A0>
+ void eval(Vector<S0, P0, A0> & res ) const {
+ SizeMismatch<3,S>::test(3, rhs.size());
+ res.template slice<0,2>()=lhs.get_rotation()*rhs.template
slice<0,2>();
+ res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
+ res[2] = rhs[2];
}
+ int size() const { return 3; }
};
-template<class Accessor> inline
-Vector<3> operator*(const SE2& lhs, const FixedVector<3,Accessor>& rhs){
- return Vector<3>(lhs,rhs,Operator<SE2VMult<FixedVector<3, Accessor> > >());
-}
-
-template<class Accessor> inline
-Vector<3> operator*(const SE2& lhs, const DynamicVector<Accessor>& rhs){
- //FIXME: size checking
- return Vector<3>(lhs,rhs,Operator<SE2VMult<DynamicVector<Accessor> > >());
+template<int S, typename P, typename PV, typename A>
+inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const
SE2<P> & lhs, const Vector<S,PV,A>& rhs){
+ return Vector<3, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE2VMult<S,P,PV,A>
>(lhs,rhs));
}
-template <class Accessor> inline
-Vector<2> operator*(const SE2& lhs, const FixedVector<2,Accessor>& rhs){
+template <typename P, typename PV, typename A>
+inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const
SE2<P>& lhs, const Vector<2,PV,A>& rhs){
return lhs.get_translation() + lhs.get_rotation() * rhs;
}
-
//////////////////
// operator * //
// Vector * SE2 //
//////////////////
-template<class Accessor>
-struct VSE2Mult {
- inline static void eval(Vector<4>& ret, const FixedVector<4,Accessor>& lhs,
const SE2& rhs){
- ret.template slice<0,2>() = lhs.template slice<0,2>() * rhs.get_rotation();
- ret[2] = lhs[2];
- ret[2] += lhs.template slice<0,2>() * rhs.get_translation();
+namespace Internal {
+template<int S, typename P, typename PV, typename A>
+struct VSE2Mult;
+}
+
+template<int S, typename P, typename PV, typename A>
+struct Operator<Internal::VSE2Mult<S,P,PV,A> > {
+ const Vector<S,PV,A> & lhs;
+ const SE2<P> & rhs;
+
+ Operator(const Vector<S,PV,A> & l, const SE2<P> & r ) : lhs(l), rhs(r)
{}
+
+ template <int S0, typename P0, typename A0>
+ void eval(Vector<S0, P0, A0> & res ) const {
+ SizeMismatch<3,S>::test(3, lhs.size());
+ res.template slice<0,2>() = lhs.template
slice<0,2>()*rhs.get_rotation();
+ res[2] = lhs[2];
+ res[2] += lhs.template slice<0,2>() * rhs.get_translation();
}
+ int size() const { return 3; }
};
-template<class Accessor> inline
-Vector<4> operator*(const FixedVector<4,Accessor>& lhs, const SE2& rhs){
- return Vector<4>(lhs,rhs,Operator<VSE2Mult<Accessor> >());
+template<int S, typename P, typename PV, typename A>
+inline Vector<3, typename Internal::MultiplyType<PV,P>::type> operator*(const
Vector<S,PV,A>& lhs, const SE2<P> & rhs){
+ return Vector<3, typename
Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSE2Mult<S, P,PV,A>
>(lhs,rhs));
}
-
-
//////////////////
// operator * //
// SE2 * Matrix //
//////////////////
-template <int RHS, class Accessor>
-struct SE2MMult {
- inline static void eval(Matrix<4,RHS>& ret, const SE2& lhs, const
FixedMatrix<4,RHS,Accessor>& rhs){
- for(int i=0; i<RHS; i++){
- ret.T()[i].template slice<0,2>() = lhs.get_rotation() *
rhs.T()[i].template slice<0,2>();
- ret.T()[i].template slice<0,2>() += lhs.get_translation() * rhs(2,i);
- ret(2,i) = rhs(2,i);
- }
+namespace Internal {
+template <int R, int C, typename PM, typename A, typename P>
+struct SE2MMult;
+}
+
+template<int R, int Cols, typename PM, typename A, typename P>
+struct Operator<Internal::SE2MMult<R, Cols, PM, A, P> > {
+ const SE2<P> & lhs;
+ const Matrix<R,Cols,PM,A> & rhs;
+
+ Operator(const SE2<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l),
rhs(r) {}
+
+ template <int R0, int C0, typename P0, typename A0>
+ void eval(Matrix<R0, C0, P0, A0> & res ) const {
+ SizeMismatch<3,R>::test(3, rhs.num_rows());
+ for(int i=0; i<rhs.num_cols(); ++i)
+ res.T()[i] = lhs * rhs.T()[i];
}
+ int num_cols() const { return rhs.num_cols(); }
+ int num_rows() const { return 3; }
};
-
-template <int RHS, class Accessor> inline
-Matrix<4,RHS> operator*(const SE2& lhs, const FixedMatrix<4,RHS,Accessor>&
rhs){
- return Matrix<4,RHS>(lhs,rhs,Operator<SE2MMult<RHS,Accessor> >());
+template <int R, int Cols, typename PM, typename A, typename P>
+inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type>
operator*(const SE2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
+ return Matrix<3,Cols,typename
Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE2MMult<R, Cols, PM, A,
P> >(lhs,rhs));
}
-
//////////////////
// operator * //
// Matrix * SE2 //
//////////////////
-template <int LHS, class Accessor>
-struct MSE2Mult {
- inline static void eval(Matrix<LHS,4>& ret, const
FixedMatrix<LHS,4,Accessor>& lhs, const SE2& rhs){
- for(int i=0; i<LHS; i++){
- ret[i].template slice<0,2>() = lhs[i].template slice<0,2>() *
rhs.get_rotation();
- ret(i,2) = rhs.get_translation() * lhs[i].template slice<0,2>();
- ret(i,2) += lhs(i,2);
- }
+namespace Internal {
+template <int Rows, int C, typename PM, typename A, typename P>
+struct MSE2Mult;
+}
+
+template<int Rows, int C, typename PM, typename A, typename P>
+struct Operator<Internal::MSE2Mult<Rows, C, PM, A, P> > {
+ const Matrix<Rows,C,PM,A> & lhs;
+ const SE2<P> & rhs;
+
+ Operator( const Matrix<Rows,C,PM,A> & l, const SE2<P> & r ) : lhs(l),
rhs(r) {}
+
+ template <int R0, int C0, typename P0, typename A0>
+ void eval(Matrix<R0, C0, P0, A0> & res ) const {
+ SizeMismatch<3, C>::test(3, lhs.num_cols());
+ for(int i=0; i<lhs.num_rows(); ++i)
+ res[i] = lhs[i] * rhs;
}
+ int num_cols() const { return 3; }
+ int num_rows() const { return lhs.num_rows(); }
};
-
-template <int LHS, class Accessor> inline
-Matrix<LHS,4> operator*(const FixedMatrix<LHS,4,Accessor>& lhs, const SE2&
rhs){
- return Matrix<LHS,4>(lhs,rhs,Operator<MSE2Mult<LHS,Accessor> >());
-}
-
-
-namespace SE2static
-{
- static double zero[2]={0,0};
+template <int Rows, int C, typename PM, typename A, typename P>
+inline Matrix<Rows,3, typename Internal::MultiplyType<PM,P>::type>
operator*(const Matrix<Rows,C,PM, A>& lhs, const SE2<P> & rhs ){
+ return Matrix<Rows,3,typename
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE2Mult<Rows, C, PM, A,
P> >(lhs,rhs));
}
-inline SE2::SE2() :
- my_translation(SE2static::zero)
-{}
-
-
/* inline SE2 SE2::exp(const Vector<3>& mu){ */
/* SE2 result; */
/* double theta = mu[2]; */
@@ -233,98 +261,60 @@
/* return result; */
/* } */
-inline SE2 SE2::exp(const Vector<3>& mu)
+template <typename Precision>
+template <int S, typename PV, typename Accessor>
+inline SE2<Precision> SE2<Precision>::exp(const Vector<S, PV, Accessor>& mu)
{
- static const double one_6th = 1.0/6.0;
- static const double one_20th = 1.0/20.0;
+ SizeMismatch<3,S>::test(3, mu.size());
+
+ static const Precision one_6th = 1.0/6.0;
+ static const Precision one_20th = 1.0/20.0;
- SE2 result;
+ SE2<Precision> result;
- const double theta = mu[2];
- const double theta_sq = theta * theta;
+ const Precision theta = mu[2];
+ const Precision theta_sq = theta * theta;
- Vector<2> cross;
- cross[0] = -theta * mu[1];
- cross[1] = theta * mu[0];
-
- result.my_rotation = SO2::exp(theta);
-
- if (theta_sq < 1e-8)
- {
- result.get_translation() = mu.slice<0,2>() + 0.5 * cross;
- }
- else
- {
- double A, B, C;
- if (theta_sq < 1e-6)
- {
- C = one_6th*(1.0 - one_20th * theta_sq);
- A = 1.0 - theta_sq * C;
+ const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta *
mu[0]);
+ result.get_rotation() = SO2<Precision>::exp(theta);
+
+ if (theta_sq < 1e-8){
+ result.get_translation() = mu.template slice<0,2>() + 0.5 *
cross;
+ } else {
+ Precision A, B;
+ if (theta_sq < 1e-6) {
+ A = 1.0 - theta_sq * one_6th*(1.0 - one_20th *
theta_sq);
B = 0.5 - 0.25 * one_6th * theta_sq;
- }
- else
- {
- const double inv_theta = (1.0/theta);
- double sine = result.my_rotation.get_matrix()[1][0];
- double cosine = result.my_rotation.get_matrix()[0][0];
+ } else {
+ const Precision inv_theta = (1.0/theta);
+ const Precision sine =
result.my_rotation.get_matrix()[1][0];
+ const Precision cosine =
result.my_rotation.get_matrix()[0][0];
A = sine * inv_theta;
B = (1 - cosine) * (inv_theta * inv_theta);
- C = (1 - A) * (inv_theta * inv_theta);
}
- result.get_translation() = (1.0 - C * theta_sq) * mu.slice<0,2>() + B *
cross;
+ result.get_translation() = A * mu.template slice<0,2>() + B *
cross;
}
return result;
}
+template <typename Precision>
+inline Vector<3, Precision> SE2<Precision>::ln(const SE2<Precision> & se2) {
+ const Precision theta = se2.get_rotation().ln();
-inline Vector<3> SE2::ln(const SE2& se2) {
- double theta = se2.my_rotation.ln();
- double shtot = 0.5;
-
- if(fabs(theta) > 0.00001) {
+ Precision shtot = 0.5;
+ if(fabs(theta) > 0.00001)
shtot = sin(theta/2)/theta;
- }
- // now do the rotation
- SO2 halfrotator = SO2::exp(-0.5 * theta );
- Vector<2> rottrans = halfrotator * se2.my_translation;
- rottrans /= (2 * shtot);
-
- Vector<3> result;
- result.slice<0,2>()=rottrans;
+ const SO2<Precision> halfrotator(theta * -0.5);
+ Vector<3, Precision> result;
+ result.template slice<0,2>() = (halfrotator * se2.get_translation())/(2
* shtot);
result[2] = theta;
return result;
}
-inline SE2 SE2::inverse() const {
- const SO2& rinv = my_rotation.inverse();
- return SE2(rinv, -(rinv*my_translation));
-}
-
-inline SE2& SE2::left_multiply_by(const SE2& left) {
- my_translation = left.my_translation + left.get_rotation() *
my_translation;
- my_rotation = left.my_rotation * my_rotation;
- return *this;
-}
-
-inline Vector<3> SE2::generator_field(int i, Vector<3> pos){
- double result_d[]={0,0,0};
- Vector<3> result(result_d);
- if(i < 2){
- result[i]=pos[2];
- return result;
- }
- result[0] = - pos[1];
- result[1] = pos[0];
- return result;
-}
-
-
-inline SE2 operator*(const SO2& lhs, const SE2& rhs){
- SE2 result;
- result.my_rotation = lhs*rhs.my_rotation;
- result.my_translation = lhs*rhs.my_translation;
- return result;
+template <typename Precision>
+inline SE2<Precision> operator*(const SO2<Precision> & lhs, const
SE2<Precision>& rhs){
+ return SE2<Precision>( lhs*rhs.get_rotation(),
lhs*rhs.get_translation());
}
#ifndef TOON_NO_NAMESPACE
Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.17
retrieving revision 1.18
diff -u -b -r1.17 -r1.18
--- se3.h 30 Jun 2008 17:58:25 -0000 1.17
+++ se3.h 2 Apr 2009 13:36:04 -0000 1.18
@@ -26,50 +26,355 @@
namespace TooN {
#endif
+template <typename Precision = double>
class SE3 {
- friend SE3 operator*(const SO3& lhs, const SE3& rhs);
- friend std::istream& operator>> (std::istream& is, SE3& rhs);
-
public:
- inline SE3();
- template <class A> inline SE3(const SO3& R, const FixedVector<3,A>& T) :
my_rotation(R), my_translation(T) {}
+ inline SE3() : my_translation(Zero) {}
+
+ template <int S, typename P, typename A>
+ SE3(const SO3<Precision> & R, const Vector<S, P, A>& T) :
my_rotation(R), my_translation(T) {}
+ template <int S, typename P, typename A>
+ SE3(const Vector<S, P, A> & v) { *this = exp(v); }
+
+ inline SO3<Precision>& get_rotation(){return my_rotation;}
+ inline const SO3<Precision>& get_rotation() const {return my_rotation;}
+ inline Vector<3, Precision>& get_translation() {return my_translation;}
+ inline const Vector<3, Precision>& get_translation() const {return
my_translation;}
+
+ template <int S, typename P, typename A>
+ static inline SE3 exp(const Vector<S, P, A>& vect);
+ static inline Vector<6, Precision> ln(const SE3& se3);
+ inline Vector<6, Precision> ln() const { return SE3::ln(*this); }
+
+ inline SE3 inverse() const {
+ const SO3<Precision> rinv = get_rotation().inverse();
+ return SE3(rinv, -(rinv*my_translation));
+ }
+
+ inline SE3& operator *=(const SE3& rhs) {
+ get_translation() += get_rotation() * rhs.get_translation();
+ get_rotation() *= rhs.get_rotation();
+ return *this;
+ }
+ inline SE3 operator *(const SE3& rhs) const { return
SE3(get_rotation()*rhs.get_rotation(), get_translation() +
get_rotation()*rhs.get_translation()); }
+
+ inline SE3& left_multiply_by(const SE3& left) {
+ get_translation() = left.get_translation() +
left.get_rotation() * get_translation();
+ get_rotation() = left.get_rotation() * get_rotation();
+ return *this;
+ }
+ static inline Matrix<4,4,Precision> generator(int i){
+ Matrix<4,4,Precision> result(Zero);
+ if(i < 3){
+ result[i][3]=1;
+ return result;
+ }
+ result[(i+1)%3][(i+2)%3] = -1;
+ result[(i+2)%3][(i+1)%3] = 1;
+ return result;
+ }
- inline SO3& get_rotation(){return my_rotation;}
- inline const SO3& get_rotation() const {return my_rotation;}
- inline Vector<3>& get_translation() {return my_translation;}
- inline const Vector<3>& get_translation() const {return my_translation;}
+ template<int S, typename Accessor>
+ inline Vector<6, Precision> adjoint(const Vector<S,Precision,
Accessor>& vect)const;
- static inline SE3 exp(const Vector<6>& vect);
- static inline Vector<6> ln(const SE3& se3);
- inline Vector<6> ln() const { return SE3::ln(*this); }
+ template<int S, typename Accessor>
+ inline Vector<6, Precision> trinvadjoint(const
Vector<S,Precision,Accessor>& vect)const;
- inline SE3 inverse() const;
+ template <int R, int C, typename Accessor>
+ inline Matrix<6,6,Precision> adjoint(const
Matrix<R,C,Precision,Accessor>& M)const;
- inline SE3& operator *=(const SE3& rhs);
- inline SE3 operator *(const SE3& rhs) const { return
SE3(my_rotation*rhs.my_rotation, my_translation +
my_rotation*rhs.my_translation); }
- inline SE3& left_multiply_by(const SE3& left);
+ template <int R, int C, typename Accessor>
+ inline Matrix<6,6,Precision> trinvadjoint(const
Matrix<R,C,Precision,Accessor>& M)const;
- static inline Vector<4> generator_field(int i, Vector<4> pos);
+private:
+ SO3<Precision> my_rotation;
+ Vector<3, Precision> my_translation;
+};
+// transfers a vector in the Lie algebra
+// from one coord frame to another
+// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
+template<typename Precision>
+template<int S, typename Accessor>
+inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,Precision,
Accessor>& vect) const{
+ SizeMismatch<6,S>::test(6, vect.size());
+ Vector<6, Precision> result;
+ result.template slice<3,3>() = get_rotation() * vect.template
slice<3,3>();
+ result.template slice<0,3>() = get_rotation() * vect.template
slice<0,3>();
+ result.template slice<0,3>() += get_translation() ^ result.template
slice<3,3>();
+ return result;
+}
- template<class Accessor>
- inline void adjoint(FixedVector<6,Accessor>& vect)const;
+// tansfers covectors between frames
+// (using the transpose of the inverse of the adjoint)
+// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
+template<typename Precision>
+template<int S, typename Accessor>
+inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const
Vector<S,Precision, Accessor>& vect) const{
+ SizeMismatch<6,S>::test(6, vect.size());
+ Vector<6, Precision> result;
+ result.template slice<3,3>() = get_rotation() * vect.template
slice<3,3>();
+ result.template slice<0,3>() = get_rotation() * vect.template
slice<0,3>();
+ result.template slice<3,3>() += get_translation() ^ result.template
slice<0,3>();
+ return result;
+}
- template<class Accessor>
- inline void trinvadjoint(FixedVector<6,Accessor>& vect)const;
+template<typename Precision>
+template<int R, int C, typename Accessor>
+inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const
Matrix<R,C,Precision,Accessor>& M)const{
+ SizeMismatch<6,R>::test(6, M.num_cols());
+ SizeMismatch<6,C>::test(6, M.num_rows());
- template <class Accessor>
- inline void adjoint(FixedMatrix<6,6,Accessor>& M)const;
+ Matrix<6,6,Precision> result;
+ for(int i=0; i<6; i++){
+ result.T()[i] = adjoint(M.T()[i]);
+ }
+ for(int i=0; i<6; i++){
+ result[i] = adjoint(result[i]);
+ }
+ return result;
+}
- template <class Accessor>
- inline void trinvadjoint(FixedMatrix<6,6,Accessor>& M)const;
+template<typename Precision>
+template<int R, int C, typename Accessor>
+inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const
Matrix<R,C,Precision,Accessor>& M)const{
+ SizeMismatch<6,R>::test(6, M.num_cols());
+ SizeMismatch<6,C>::test(6, M.num_rows());
-private:
- SO3 my_rotation;
- Vector<3> my_translation;
+ Matrix<6,6,Precision> result;
+ for(int i=0; i<6; i++){
+ result.T()[i] = trinvadjoint(M.T()[i]);
+ }
+ for(int i=0; i<6; i++){
+ result[i] = trinvadjoint(result[i]);
+ }
+ return result;
+}
+
+// operator ostream& <<
+template <typename Precision>
+inline std::ostream& operator <<(std::ostream& os, const SE3<Precision>& rhs){
+ for(int i=0; i<3; i++){
+ os << rhs.get_rotation().get_matrix()[i] <<
rhs.get_translation()[i] << std::endl;
+ }
+ return os;
+}
+
+// operator istream& >>
+template <typename Precision>
+inline std::istream& operator>>(std::istream& is, SE3<Precision>& rhs){
+ for(int i=0; i<3; i++){
+ is >> rhs.get_rotation().my_matrix[i].ref() >>
rhs.get_translation()[i];
+ }
+ return is;
+}
+
+//////////////////
+// operator * //
+// SE3 * Vector //
+//////////////////
+
+namespace Internal {
+template<int S, typename PV, typename A, typename P>
+struct SE3VMult;
+}
+
+template<int S, typename PV, typename A, typename P>
+struct Operator<Internal::SE3VMult<S,PV,A,P> > {
+ const SE3<P> & lhs;
+ const Vector<S,PV,A> & rhs;
+
+ Operator(const SE3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r)
{}
+
+ template <int S0, typename P0, typename A0>
+ void eval(Vector<S0, P0, A0> & res ) const {
+ SizeMismatch<4,S>::test(4, rhs.size());
+ res.template slice<0,3>()=lhs.get_rotation() * rhs.template
slice<0,3>();
+ res.template slice<0,3>()+=lhs.get_translation() * rhs[3];
+ res[3] = rhs[3];
+ }
+ int size() const { return 4; }
+};
+
+template<int S, typename PV, typename A, typename P> inline
+Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>
& lhs, const Vector<S,PV,A>& rhs){
+ return Vector<4, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE3VMult<S,PV,A,P>
>(lhs,rhs));
+}
+
+template <typename PV, typename A, typename P> inline
+Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE3<P>&
lhs, const Vector<3,PV,A>& rhs){
+ return lhs.get_translation() + lhs.get_rotation() * rhs;
+}
+
+//////////////////
+// operator * //
+// Vector * SE3 //
+//////////////////
+
+namespace Internal {
+template<int S, typename PV, typename A, typename P>
+struct VSE3Mult;
+}
+
+template<int S, typename PV, typename A, typename P>
+struct Operator<Internal::VSE3Mult<S,PV,A,P> > {
+ const Vector<S,PV,A> & lhs;
+ const SE3<P> & rhs;
+
+ Operator( const Vector<S,PV,A> & l, const SE3<P> & r ) : lhs(l), rhs(r)
{}
+
+ template <int S0, typename P0, typename A0>
+ void eval(Vector<S0, P0, A0> & res ) const {
+ SizeMismatch<4,S>::test(4, lhs.size());
+ res.template slice<0,3>()=lhs.template slice<0,3>() *
rhs.get_rotation();
+ res[3] = lhs[3];
+ res[3] += lhs.template slice<0,3>() * rhs.get_translation();
+ }
+ int size() const { return 4; }
+};
+
+template<int S, typename PV, typename A, typename P> inline
+Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const
Vector<S,PV,A>& lhs, const SE3<P> & rhs){
+ return Vector<4, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSE3Mult<S,PV,A,P>
>(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// SE3 * Matrix //
+//////////////////
+
+namespace Internal {
+template <int R, int C, typename PM, typename A, typename P>
+struct SE3MMult;
+}
+
+template<int R, int Cols, typename PM, typename A, typename P>
+struct Operator<Internal::SE3MMult<R, Cols, PM, A, P> > {
+ const SE3<P> & lhs;
+ const Matrix<R,Cols,PM,A> & rhs;
+
+ Operator(const SE3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l),
rhs(r) {}
+
+ template <int R0, int C0, typename P0, typename A0>
+ void eval(Matrix<R0, C0, P0, A0> & res ) const {
+ SizeMismatch<4,R>::test(4, rhs.num_rows());
+ for(int i=0; i<rhs.num_cols(); ++i)
+ res.T()[i] = lhs * rhs.T()[i];
+ }
+ int num_cols() const { return rhs.num_cols(); }
+ int num_rows() const { return 4; }
+};
+
+template <int R, int Cols, typename PM, typename A, typename P> inline
+Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const
SE3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
+ return Matrix<4,Cols,typename
Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE3MMult<R, Cols, PM, A,
P> >(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// Matrix * SE3 //
+//////////////////
+
+namespace Internal {
+template <int Rows, int C, typename PM, typename A, typename P>
+struct MSE3Mult;
+}
+
+template<int Rows, int C, typename PM, typename A, typename P>
+struct Operator<Internal::MSE3Mult<Rows, C, PM, A, P> > {
+ const Matrix<Rows,C,PM,A> & lhs;
+ const SE3<P> & rhs;
+
+ Operator( const Matrix<Rows,C,PM,A> & l, const SE3<P> & r ) : lhs(l),
rhs(r) {}
+
+ template <int R0, int C0, typename P0, typename A0>
+ void eval(Matrix<R0, C0, P0, A0> & res ) const {
+ SizeMismatch<4, C>::test(4, lhs.num_cols());
+ for(int i=0; i<lhs.num_rows(); ++i)
+ res[i] = lhs[i] * rhs;
+ }
+ int num_cols() const { return 4; }
+ int num_rows() const { return lhs.num_rows(); }
};
+template <int Rows, int C, typename PM, typename A, typename P> inline
+Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const
Matrix<Rows,C,PM, A>& lhs, const SE3<P> & rhs ){
+ return Matrix<Rows,4,typename
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE3Mult<Rows, C, PM, A,
P> >(lhs,rhs));
+}
+
+template <typename Precision>
+template <int S, typename P, typename VA>
+inline SE3<Precision> SE3<Precision>::exp(const Vector<S, P, VA>& mu){
+ SizeMismatch<6,S>::test(6, mu.size());
+ static const Precision one_6th = 1.0/6.0;
+ static const Precision one_20th = 1.0/20.0;
+
+ SE3<Precision> result;
+
+ const Vector<3,Precision> w = mu.template slice<3,3>();
+ const Precision theta_sq = w*w;
+ const Precision theta = sqrt(theta_sq);
+ double A, B;
+
+ const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
+ if (theta_sq < 1e-8) {
+ A = 1.0 - one_6th * theta_sq;
+ B = 0.5;
+ result.get_translation() = mu.template slice<0,3>() + 0.5 *
cross;
+ } else {
+ double C;
+ if (theta_sq < 1e-6) {
+ C = one_6th*(1.0 - one_20th * theta_sq);
+ A = 1.0 - theta_sq * C;
+ B = 0.5 - 0.25 * one_6th * theta_sq;
+ } else {
+ const double inv_theta = 1.0/theta;
+ A = sin(theta) * inv_theta;
+ B = (1 - cos(theta)) * (inv_theta * inv_theta);
+ C = (1 - A) * (inv_theta * inv_theta);
+ }
+ result.get_translation() = mu.template slice<0,3>() + B * cross
+ C * (w ^ cross);
+ }
+ rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
+ return result;
+}
+
+template <typename Precision>
+inline Vector<6, Precision> SE3<Precision>::ln(const SE3<Precision>& se3) {
+ Vector<3,Precision> rot = se3.get_rotation().ln();
+ const Precision theta = sqrt(rot*rot);
+
+ Precision shtot = 0.5;
+ if(theta > 0.00001) {
+ shtot = sin(theta/2)/theta;
+ }
+
+ // now do the rotation
+ const SO3<Precision> halfrotator = SO3<Precision>::exp(rot * -0.5);
+ Vector<3, Precision> rottrans = halfrotator * se3.get_translation();
+
+ if(theta > 0.001){
+ rottrans -= rot * ((se3.get_translation() * rot) * (1-2*shtot)
/ (rot*rot));
+ } else {
+ rottrans -= rot * ((se3.get_translation() * rot)/24);
+ }
+
+ rottrans /= (2 * shtot);
+
+ Vector<6, Precision> result;
+ result.template slice<0,3>()=rottrans;
+ result.template slice<3,3>()=rot;
+ return result;
+}
+
+template <typename Precision>
+inline SE3<Precision> operator*(const SO3<Precision>& lhs, const
SE3<Precision>& rhs){
+ return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
+}
+
+#if 0 // should be moved to another header file
+
template <class A> inline
Vector<3> transform(const SE3& pose, const FixedVector<3,A>& x) { return
pose.get_rotation()*x + pose.get_translation(); }
@@ -236,272 +541,7 @@
return uv;
}
-
-
-
-// left multiply an SE3 by an SO3
-inline SE3 operator*(const SO3& lhs, const SE3& rhs);
-
-
-// transfers a vector in the Lie algebra
-// from one coord frame to another
-// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
-template<class Accessor>
-inline void SE3::adjoint(FixedVector<6,Accessor>& vect)const{
- vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
- vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
- vect.template slice<0,3>() += my_translation ^ vect.template slice<3,3>();
-}
-
-// tansfers covectors between frames
-// (using the transpose of the inverse of the adjoint)
-// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
-template<class Accessor>
-inline void SE3::trinvadjoint(FixedVector<6,Accessor>& vect)const{
- vect.template slice<3,3>() = my_rotation * vect.template slice<3,3>();
- vect.template slice<0,3>() = my_rotation * vect.template slice<0,3>();
- vect.template slice<3,3>() += my_translation ^ vect.template slice<0,3>();
-}
-
-template <class Accessor>
-inline void SE3::adjoint(FixedMatrix<6,6,Accessor>& M)const{
- for(int i=0; i<6; i++){
- adjoint(M.T()[i]);
- }
- for(int i=0; i<6; i++){
- adjoint(M[i]);
- }
-}
-
-template <class Accessor>
-inline void SE3::trinvadjoint(FixedMatrix<6,6,Accessor>& M)const{
- for(int i=0; i<6; i++){
- trinvadjoint(M.T()[i]);
- }
- for(int i=0; i<6; i++){
- trinvadjoint(M[i]);
- }
-}
-
-
-// operator ostream& <<
-inline std::ostream& operator <<(std::ostream& os, const SE3& rhs){
- for(int i=0; i<3; i++){
- os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] <<
std::endl;
- }
- return os;
-}
-
-// operator istream& >>
-inline std::istream& operator>>(std::istream& is, SE3& rhs){
- for(int i=0; i<3; i++){
- is >> rhs.get_rotation().my_matrix[i] >> rhs.get_translation()[i];
- }
- return is;
-}
-
-
-//////////////////
-// operator * //
-// SE3 * Vector //
-//////////////////
-
-template<class VectorType>
-struct SE3VMult {
- inline static void eval(Vector<4>& ret, const SE3& lhs, const VectorType&
rhs){
- ret.template slice<0,3>()=lhs.get_rotation()*rhs.template slice<0,3>();
- ret.template slice<0,3>()+=lhs.get_translation() * rhs[3];
- ret[3] = rhs[3];
- }
-};
-
-template<class Accessor> inline
-Vector<4> operator*(const SE3& lhs, const FixedVector<4,Accessor>& rhs){
- return Vector<4>(lhs,rhs,Operator<SE3VMult<FixedVector<4, Accessor> > >());
-}
-
-template<class Accessor> inline
-Vector<4> operator*(const SE3& lhs, const DynamicVector<Accessor>& rhs){
- //FIXME: size checking
- return Vector<4>(lhs,rhs,Operator<SE3VMult<DynamicVector<Accessor> > >());
-}
-
-template <class Accessor> inline
-Vector<3> operator*(const SE3& lhs, const FixedVector<3,Accessor>& rhs){
- return lhs.get_rotation()*rhs + lhs.get_translation();
-}
-
-
-//////////////////
-// operator * //
-// Vector * SE3 //
-//////////////////
-
-template<class Accessor>
-struct VSE3Mult {
- inline static void eval(Vector<4>& ret, const FixedVector<4,Accessor>&
lhs, const SE3& rhs){
- ret.template slice<0,3>() = lhs.template slice<0,3>() *
rhs.get_rotation();
- ret[3] = lhs[3];
- ret[3] += lhs.template slice<0,3>() * rhs.get_translation();
- }
-};
-
-template<class Accessor> inline
-Vector<4> operator*(const FixedVector<4,Accessor>& lhs, const SE3& rhs){
- return Vector<4>(lhs,rhs,Operator<VSE3Mult<Accessor> >());
-}
-
-
-
-//////////////////
-// operator * //
-// SE3 * Matrix //
-//////////////////
-
-template <int RHS, class Accessor>
-struct SE3MMult {
- inline static void eval(Matrix<4,RHS>& ret, const SE3& lhs, const
FixedMatrix<4,RHS,Accessor>& rhs){
- for(int i=0; i<RHS; i++){
- ret.T()[i].template slice<0,3>() = lhs.get_rotation() *
rhs.T()[i].template slice<0,3>();
- ret.T()[i].template slice<0,3>() += lhs.get_translation() *
rhs(3,i);
- ret(3,i) = rhs(3,i);
- }
- }
-};
-
-
-template <int RHS, class Accessor> inline
-Matrix<4,RHS> operator*(const SE3& lhs, const FixedMatrix<4,RHS,Accessor>&
rhs){
- return Matrix<4,RHS>(lhs,rhs,Operator<SE3MMult<RHS,Accessor> >());
-}
-
-
-//////////////////
-// operator * //
-// Matrix * SE3 //
-//////////////////
-
-template <int LHS, class Accessor>
-struct MSE3Mult {
- inline static void eval(Matrix<LHS,4>& ret, const
FixedMatrix<LHS,4,Accessor>& lhs, const SE3& rhs){
- for(int i=0; i<LHS; i++){
- ret[i].template slice<0,3>() = lhs[i].template slice<0,3>() *
rhs.get_rotation();
- ret(i,3) = rhs.get_translation() * lhs[i].template slice<0,3>();
- ret(i,3) += lhs(i,3);
- }
- }
-};
-
-
-template <int LHS, class Accessor> inline
-Matrix<LHS,4> operator*(const FixedMatrix<LHS,4,Accessor>& lhs, const SE3&
rhs){
- return Matrix<LHS,4>(lhs,rhs,Operator<MSE3Mult<LHS,Accessor> >());
-}
-
-
-namespace SE3static
-{
- static double zero[3]={0,0,0};
-}
-
-inline SE3::SE3() :
- my_translation(SE3static::zero)
-{}
-
-
-inline SE3 SE3::exp(const Vector<6>& mu){
- static const double one_6th = 1.0/6.0;
- static const double one_20th = 1.0/20.0;
-
- SE3 result;
-
- const Vector<3> w = mu.slice<3,3>();
- const double theta_sq = w*w;
- const double theta = sqrt(theta_sq);
- double A, B;
-
- const Vector<3> cross = w ^ mu.slice<0,3>();
- if (theta_sq < 1e-8) {
- A = 1.0 - one_6th * theta_sq;
- B = 0.5;
- result.get_translation() = mu.slice<0,3>() + 0.5 * cross;
- } else {
- double C;
- if (theta_sq < 1e-6) {
- C = one_6th*(1.0 - one_20th * theta_sq);
- A = 1.0 - theta_sq * C;
- B = 0.5 - 0.25 * one_6th * theta_sq;
- } else {
- const double inv_theta = 1.0/theta;
- A = sin(theta) * inv_theta;
- B = (1 - cos(theta)) * (inv_theta * inv_theta);
- C = (1 - A) * (inv_theta * inv_theta);
- }
- result.get_translation() = mu.slice<0,3>() + B * cross + C * (w ^
cross);
- }
- rodrigues_so3_exp(w, A, B, result.my_rotation.my_matrix);
- return result;
-}
-
-inline Vector<6> SE3::ln(const SE3& se3) {
- Vector<3> rot = se3.my_rotation.ln();
- double theta = sqrt(rot*rot);
- double shtot = 0.5;
-
- if(theta > 0.00001) {
- shtot = sin(theta/2)/theta;
- }
-
- // now do the rotation
- Vector<3> halfrot = rot * -0.5;
- SO3 halfrotator = SO3::exp(halfrot);
-
- Vector<3> rottrans = halfrotator * se3.my_translation;
-
- if(theta > 0.001){
- rottrans -= rot * ((se3.my_translation * rot) * (1-2*shtot) /
(rot*rot));
- } else {
- rottrans -= rot * ((se3.my_translation * rot)/24);
- }
-
- rottrans /= (2 * shtot);
-
- Vector<6> result;
- result.slice<0,3>()=rottrans;
- result.slice<3,3>()=rot;
- return result;
-}
-
-inline SE3 SE3::inverse() const {
- const SO3& rinv = my_rotation.inverse();
- return SE3(rinv, -(rinv*my_translation));
-}
-
-inline SE3& SE3::left_multiply_by(const SE3& left) {
- my_translation = left.my_translation + left.get_rotation() *
my_translation;
- my_rotation = left.my_rotation * my_rotation;
- return *this;
-}
-
-inline Vector<4> SE3::generator_field(int i, Vector<4> pos){
- double result_d[]={0,0,0,0};
- Vector<4> result(result_d);
- if(i < 3){
- result[i]=pos[3];
- return result;
- }
- result[(i+1)%3] = - pos[(i+2)%3];
- result[(i+2)%3] = pos[(i+1)%3];
- return result;
-}
-
-
-inline SE3 operator*(const SO3& lhs, const SE3& rhs){
- SE3 result;
- result.my_rotation = lhs*rhs.my_rotation;
- result.my_translation = lhs*rhs.my_translation;
- return result;
-}
+#endif
#ifndef TOON_NO_NAMESPACE
}
Index: so2.h
===================================================================
RCS file: /cvsroot/toon/TooN/so2.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- so2.h 26 Mar 2009 19:28:30 -0000 1.5
+++ so2.h 2 Apr 2009 13:36:04 -0000 1.6
@@ -27,28 +27,32 @@
namespace TooN {
#endif
-template<class Precision> class SO2;
+template<typename Precision> class SO2;
+template <typename Precision> class SE2;
-template<class Precision> inline std::istream & operator>>(std::istream &,
SO2<Precision> & );
+template<typename Precision> inline std::istream & operator>>(std::istream &,
SO2<Precision> & );
+template<typename Precision> inline std::istream & operator>>(std::istream &,
SE2<Precision> & );
-template<class Precision = double>
+template<typename Precision = double>
class SO2 {
-public:
friend std::istream& operator>> <Precision>(std::istream&, SO2& );
+ friend std::istream& operator>> <Precision>(std::istream&,
SE2<Precision>& );
+
+public:
SO2() : my_matrix(Identity) {}
SO2(const Matrix<2,2,Precision>& rhs) { *this = rhs; }
SO2(const Precision l) { *this = exp(l); }
- template <int R, int C, class P, class A>
+ template <int R, int C, typename P, typename A>
inline SO2& operator=(const Matrix<R,C,P,A>& rhs){
my_matrix = rhs;
coerce(my_matrix);
return *this;
}
- template <int R, int C, class P, class A>
+ template <int R, int C, typename P, typename A>
static inline void coerce(Matrix<R,C,P,A>& M){
SizeMismatch<2,R>::test(2, M.num_rows());
SizeMismatch<2,C>::test(2, M.num_cols());
@@ -93,32 +97,33 @@
Matrix<2,2,Precision> my_matrix;
};
-template <class Precision>
+template <typename Precision>
inline std::ostream& operator<< (std::ostream& os, const SO2<Precision> & rhs){
return os << rhs.get_matrix();
}
-template <class Precision>
+template <typename Precision>
inline std::istream& operator>>(std::istream& is, SO2<Precision>& rhs){
return is >> rhs.my_matrix;
+ SO2<Precision>::coerce(rhs.my_matrix);
}
-template<int D, class P1, class PV, class Accessor>
+template<int D, typename P1, typename PV, typename Accessor>
inline Vector<2, typename Internal::MultiplyType<P1, PV>::type>
operator*(const SO2<P1> & lhs, const Vector<D, PV, Accessor> & rhs){
return lhs.get_matrix() * rhs;
}
-template<int D, class P1, class PV, class Accessor>
+template<int D, typename P1, typename PV, typename Accessor>
inline Vector<2, typename Internal::MultiplyType<PV,P1>::type> operator*(const
Vector<D, PV, Accessor>& lhs, const SO2<P1> & rhs){
return lhs * rhs.get_matrix();
}
-template <int R, int C, class P1, class P2, class Accessor>
+template <int R, int C, typename P1, typename P2, typename Accessor>
inline Matrix<2,C,typename Internal::MultiplyType<P1,P2>::type>
operator*(const SO2<P1> & lhs, const Matrix<R,C,P2,Accessor>& rhs){
return lhs.get_matrix() * rhs;
}
-template <int R, int C, class P1, class P2, class Accessor>
+template <int R, int C, typename P1, typename P2, typename Accessor>
inline Matrix<R,2,typename Internal::MultiplyType<P1,P2>::type>
operator*(const Matrix<R,C,P1,Accessor>& lhs, const SO2<P2>& rhs){
return lhs * rhs.get_matrix();
}
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.24
retrieving revision 1.25
diff -u -b -r1.24 -r1.25
--- so3.h 1 Apr 2009 13:22:13 -0000 1.24
+++ so3.h 2 Apr 2009 13:36:04 -0000 1.25
@@ -28,13 +28,17 @@
#endif
template <typename Precision> class SO3;
+template <typename Precision> class SE3;
template<class Precision> inline std::istream & operator>>(std::istream &,
SO3<Precision> & );
+template<class Precision> inline std::istream & operator>>(std::istream &,
SE3<Precision> & );
template <typename Precision = double>
class SO3 {
public:
friend std::istream& operator>> <Precision> (std::istream& is,
SO3<Precision> & rhs);
+ friend std::istream& operator>> <Precision> (std::istream& is,
SE3<Precision> & rhs);
+ friend class SE3<Precision>;
SO3() : my_matrix(Identity) {}
@@ -235,13 +239,8 @@
return lhs * rhs.get_matrix();
}
-
#if 0 // will be moved to another header file ?
-inline Vector<3> SO3::adjoint(Vector<3> vect)const {
- return my_matrix * vect;
-}
-
template <class A> inline
Vector<3> transform(const SO3& pose, const FixedVector<3,A>& x) { return
pose*x; }
Index: test/SXX_test.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/SXX_test.cc,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- test/SXX_test.cc 1 Apr 2009 13:21:26 -0000 1.5
+++ test/SXX_test.cc 2 Apr 2009 13:36:04 -0000 1.6
@@ -1,10 +1,12 @@
#include <iostream>
+#include <sstream>
using namespace std;
#include <TooN/so2.h>
#include <TooN/se2.h>
#include <TooN/so3.h>
+#include <TooN/se3.h>
void test_so2(){
cout << "---------------SO2 Tests---------------\n";
@@ -49,6 +51,11 @@
cout << "set from matrix (uses coerce) " << m << "\n";
r = m;
cout << r << endl;
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 1 0");
+ is >> r;
+ cout << r << endl;
}
void test_se2(){
@@ -61,6 +68,9 @@
TooN::SE2<int> r2;
cout << r2 << endl;
+ cout << "from vector 1 1 0\n";
+ cout << TooN::SE2<>::exp(TooN::makeVector(1,1,0)) << endl;
+
TooN::SE2<> r3(TooN::makeVector(1,1,1));
cout << "from vector 1 1 1\n";
cout << r3 << endl;
@@ -80,7 +90,6 @@
// cout << t1 * r3 << endl; // this is not well defined, should the output
be a 3 vector ?
cout << t2 * r3 << endl;
-#if 0
TooN::Matrix<3> m1;
TooN::Matrix<> m2(3,3);
TooN::Matrix<3,10> m3;
@@ -92,7 +101,15 @@
cout << m1 * r3 << endl;
cout << m2 * r3 << endl;
cout << m3.T() * r3 << endl;
-#endif
+
+ TooN::SO2<> r(-1);
+ cout << "so2 * se2\n";
+ cout << r * r3 << endl;
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 2 1 0 3");
+ is >> r3;
+ cout << r3 << endl;
}
void test_so3(){
@@ -141,12 +158,106 @@
cout << "set from matrix (uses coerce)\n" << m << "\n";
r = m;
cout << r << endl;
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 0 1 0 0 0 0 1");
+ is >> r;
+ cout << r << endl;
+}
+
+void test_se3(){
+ cout << "---------------SE3 Tests---------------\n";
+
+ TooN::SE3<> r1;
+ cout << "default constructor\n";
+ cout << r1 << endl;
+ cout << "default constructor <int>\n";
+ TooN::SE3<int> r2;
+ cout << r2 << endl;
+ TooN::SE3<> r(TooN::makeVector(0.1, 0.1, 0.1, 0.2, -0.2, 0.2));
+ cout << "constructor\n";
+ cout << r << endl;
+ cout << "generator 0,1,2,3,4,5\n";
+ cout << TooN::SE3<>::generator(0) ;
+ cout << TooN::SE3<>::generator(1) ;
+ cout << TooN::SE3<>::generator(2) << endl;
+ cout << TooN::SE3<>::generator(3) ;
+ cout << TooN::SE3<>::generator(4) ;
+ cout << TooN::SE3<>::generator(5) << endl;
+ cout << "ln()\n";
+ cout << r.ln() << endl;
+ cout << "inverse\n";
+ cout << r.inverse() << endl;
+ cout << "times\n";
+ cout << r * r.inverse() << endl;
+ cout << (r *= r.inverse()) << endl;
+ r = TooN::SE3<>::exp(TooN::makeVector(0.1, 0.1, 0.1, 0.2, -0.2, 0.2));
+
+ TooN::Vector<4> t = TooN::makeVector(0,1,2,3);
+ cout << "right and left multiply with vector " << t << "\n";
+ cout << r * t << endl;
+ cout << t * r << endl;
+ TooN::Vector<3> t3 = TooN::makeVector(0,1,2);
+ cout << "right with a 3 vector " << t3 << "\n";
+ cout << r * t3 << endl;
+
+ TooN::Matrix<4> l = TooN::Identity;
+ cout << "right and left multiply with matrix\n" << l << "\n";
+ cout << r * l << endl;
+ cout << l * r << endl;
+ TooN::Matrix<4,6> l2(TooN::Zero);
+ l2[0] = TooN::makeVector(0,1,2,3,4,5);
+ cout << "right with rectangular matrix\n";
+ cout << r * l2 << endl;
+ cout << l2.T() * r << endl;
+
+ TooN::SO3<> rot(TooN::makeVector(-0.2, 0.2, -0.2));
+ cout << "mult with SO3\n";
+ cout << rot * r << endl;
+
+ TooN::Vector<6> a = TooN::makeVector(0,1,2,0.1, 0.2, 0.3);
+ cout << "adjoint with a " << a << "\n";
+ cout << r.adjoint(a) << endl;
+ cout << "0 0 0 0 0 0 = " << r.adjoint(a) - (r * TooN::SE3<>(a) *
r.inverse()).ln() << endl;
+ cout << "trinvadjoint with a " << a << "\n";
+ cout << r.trinvadjoint(a) << endl;
+ cout << "0 = " << r.trinvadjoint(a) * r.adjoint(a) - a * a << endl;
+
+ TooN::Matrix<6> ma(TooN::Identity);
+ ma[0] = TooN::makeVector(0.1, 0.2, 0.1, 0.2, 0.1, 0.3);
+ ma = ma.T() * ma;
+ cout << "adjoint with ma\n" << ma << "\n";
+ cout << r.adjoint(ma) << endl;
+ cout << "trinvadjoint with ma\n";
+ cout << r.trinvadjoint(ma) << endl;
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 0 1 1 0 0 2 0 0 1 3");
+ is >> r;
+ cout << r << endl;
+}
+
+
+void test_se2_exp(){
+ cout << "------------------SE2 check------------------\n";
+
+ TooN::SE2<> s2(TooN::makeVector(1,0,1));
+ TooN::SE3<> s3(TooN::makeVector(0,1,0,1,0,0));
+ cout << s2 << endl;
+ cout << s3 << endl;
+
+ cout << s2.ln() << "\tvs\t" << s3.ln() << endl;
+
}
int main(int, char* *){
test_so2();
test_so3();
+ test_se2();
+ test_se3();
+
+ test_se2_exp();
return 0;
}
- [Toon-members] TooN se2.h se3.h so2.h so3.h test/SXX_test.cc,
Gerhard Reitmayr <=