toon-members
[Top][All Lists]
Advanced

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

[Toon-members] TooN se2.h so2.h


From: Georg Klein
Subject: [Toon-members] TooN se2.h so2.h
Date: Wed, 22 Aug 2007 14:17:17 +0000

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Georg Klein <georgklein>        07/08/22 14:17:16

Added files:
        .              : se2.h so2.h 

Log message:
        Added SE2/SO2, made mostly be copying the stuff from SE3/SO3. Exp, ln,
        multiplication and adjoint seem to work, much of the rest is pretty
        untested.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/se2.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/TooN/so2.h?cvsroot=toon&rev=1.1

Patches:
Index: se2.h
===================================================================
RCS file: se2.h
diff -N se2.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ se2.h       22 Aug 2007 14:17:15 -0000      1.1
@@ -0,0 +1,334 @@
+
+/*                       
+     Copyright (C) 2005 The Authors
+
+     This library is free software; you can redistribute it and/or
+     modify it under the terms of the GNU Lesser General Public
+     License as published by the Free Software Foundation; either
+     version 2.1 of the License, or (at your option) any later version.
+
+     This library is distributed in the hope that it will be useful,
+     but WITHOUT ANY WARRANTY; without even the implied warranty of
+     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+     Lesser General Public License for more details.
+
+     You should have received a copy of the GNU Lesser General Public
+     License along with this library; if not, write to the Free Software
+     Foundation, Inc.
+     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+/* This code mostly made by copying from se3.h !! */
+
+#ifndef __SE2_H
+#define __SE2_H
+
+#include <TooN/so2.h>
+
+#ifndef TOON_NO_NAMESPACE
+namespace TooN {
+#endif
+
+class SE2 {
+  friend SE2 operator*(const SO2& lhs, const SE2& rhs);
+  friend std::istream& operator>> (std::istream& is, SE2& rhs);
+  
+ 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;
+
+  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);
+
+
+  template<class Accessor>
+  inline void adjoint(FixedVector<3,Accessor>& vect)const;
+
+  template <class Accessor>
+  inline void adjoint(FixedMatrix<3,3,Accessor>& M)const;
+
+private:
+  SO2 my_rotation;
+  Vector<2> 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++){
+    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];
+  }
+  return is;
+}
+
+
+//////////////////
+// operator *   //
+// 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];
+  }
+};
+
+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 <class Accessor> inline
+Vector<2> operator*(const SE2& lhs, const FixedVector<2,Accessor>& 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();
+  }
+};
+
+template<class Accessor> inline
+Vector<4> operator*(const FixedVector<4,Accessor>& lhs, const SE2& rhs){
+  return Vector<4>(lhs,rhs,Operator<VSE2Mult<Accessor> >());
+}
+
+
+
+//////////////////
+// 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);
+    }
+  }
+};
+
+
+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> >());
+}
+
+
+//////////////////
+// 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);
+    }
+  }
+};
+
+
+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};
+}
+
+inline SE2::SE2() :
+  my_translation(SE2static::zero)
+{}
+
+
+/* inline SE2 SE2::exp(const Vector<3>& mu){ */
+/*   SE2 result; */
+/*   double theta = mu[2]; */
+/*   result.get_rotation() = SO2::exp(theta); */
+/*   Matrix<2> m2; */
+/*   m2[0][0] = m2[1][1] = result.get_rotation().get_matrix()[1][0]; */
+/*   m2[0][1] = result.get_rotation().get_matrix()[0][0] - 1.0; */
+/*   m2[1][0] = - m2[0][1]; */
+/*   if(theta != 0.0)  */
+/*     result.get_translation() = m2 * mu.slice<0,2>() / fabs(theta); */
+/*   else */
+/*     result.get_translation() = mu.slice<0,2>(); */
+/*   return result; */
+/* } */
+
+inline SE2 SE2::exp(const Vector<3>& mu)
+{
+  static const double one_6th = 1.0/6.0;
+  static const double one_20th = 1.0/20.0;
+  
+  SE2 result;
+  
+  const double theta = mu[2];
+  const double 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;
+         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];
+         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;
+    }
+  return result;
+}
+ 
+
+inline Vector<3> SE2::ln(const SE2& se2) {
+  double theta = se2.my_rotation.ln();
+  double 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;
+  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;
+}
+
+#ifndef TOON_NO_NAMESPACE
+}
+#endif
+
+#endif

Index: so2.h
===================================================================
RCS file: so2.h
diff -N so2.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ so2.h       22 Aug 2007 14:17:16 -0000      1.1
@@ -0,0 +1,167 @@
+/*
+     Copyright (C) 2007 The Authors
+
+     This library is free software; you can redistribute it and/or
+     modify it under the terms of the GNU Lesser General Public
+     License as published by the Free Software Foundation; either
+     version 2.1 of the License, or (at your option) any later version.
+
+     This library is distributed in the hope that it will be useful,
+     but WITHOUT ANY WARRANTY; without even the implied warranty of
+     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+     Lesser General Public License for more details.
+
+     You should have received a copy of the GNU Lesser General Public
+     License along with this library; if not, write to the Free Software
+     Foundation, Inc.
+     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+/* This code mostly made by copying from so2.h !! */
+
+#ifndef __SO2_H
+#define __SO2_H
+
+#include <TooN/TooN.h>
+#include <TooN/LU.h>
+#include <TooN/helpers.h>
+
+#ifndef TOON_NO_NAMESPACE
+namespace TooN {
+#endif
+
+    class SE2;
+
+class SO2 {
+public:
+  friend std::istream& operator>>(std::istream& is, SO2& rhs);
+  friend std::istream& operator>>(std::istream& is, class SE2& rhs);
+  friend class SE2;
+  inline SO2();
+  inline SO2(const Matrix<2>& rhs);
+
+  inline SO2& operator=(const Matrix<2>& rhs);
+  template <class Accessor> static inline void 
coerce(FixedMatrix<2,2,Accessor>& M);
+
+  inline static SO2 exp(const double);
+
+  inline double ln() const;
+
+  inline double operator[](int i){return my_matrix[i/2][i%2];}
+
+  inline SO2 inverse() const;
+
+  inline SO2& operator *=(const SO2& rhs){
+    my_matrix=my_matrix*rhs.my_matrix;
+    return *this;
+  }
+
+  inline SO2 operator *(const SO2& rhs) const {
+      return SO2(*this,rhs);
+  }
+
+  inline const Matrix<2>& get_matrix()const {return my_matrix;}
+  // returns i-th generator times pos
+  inline static Vector<2> generator(Vector<2> pos);
+
+ private:
+  struct Invert {};
+  inline SO2(const SO2& so2, const Invert&) : my_matrix(so2.my_matrix.T()) {}
+  inline SO2(const SO2& a, const SO2& b) : my_matrix(a.my_matrix*b.my_matrix) 
{}
+      
+  Matrix<2> my_matrix;
+};
+
+inline std::ostream& operator<< (std::ostream& os, const SO2& rhs){
+  return os << rhs.get_matrix();
+}
+
+inline std::istream& operator>>(std::istream& is, SO2& rhs){
+  return is >> rhs.my_matrix;
+}
+
+template<class Accessor> inline
+Vector<2> operator*(const SO2& lhs, const FixedVector<2,Accessor>& rhs){
+  return lhs.get_matrix() * rhs;
+}
+
+template<class Accessor> inline
+Vector<2> operator*(const SO2& lhs, const DynamicVector<Accessor>& rhs){
+  assert(rhs.size() == 2);
+  return lhs.get_matrix() * rhs;
+}
+
+template<class Accessor> inline
+Vector<2> operator*(const DynamicVector<Accessor>& lhs, const SO2& rhs){
+  assert(lhs.size() == 2);
+  return lhs * rhs.get_matrix();
+}
+
+template<class Accessor> inline
+Vector<2> operator*(const FixedVector<2,Accessor>& lhs, const SO2& rhs){
+  return lhs * rhs.get_matrix();
+}
+
+template <int RHS, class Accessor> inline
+Matrix<2,RHS> operator*(const SO2& lhs, const FixedMatrix<2,RHS,Accessor>& 
rhs){
+  return lhs.get_matrix() * rhs;
+}
+
+template <int LHS, class Accessor> inline
+Matrix<LHS,2> operator*(const FixedMatrix<LHS,2,Accessor>& lhs, const SO2& 
rhs){
+  return lhs * rhs.get_matrix();
+}
+
+namespace SO2static
+{
+  static double identity[4]={1,0,0,1};
+}
+
+inline SO2::SO2() :
+  my_matrix(SO2static::identity)
+{}
+
+inline SO2::SO2(const Matrix<2>& rhs){
+    *this = rhs;
+}
+
+inline SO2& SO2::operator=(const Matrix<2>& rhs){
+  my_matrix = rhs;
+  coerce(my_matrix);
+  return *this;
+}
+
+template <class Accessor> inline void SO2::coerce(FixedMatrix<2,2, Accessor>& 
M){
+  normalize(M[0]);
+  M[1] -= M[0] * (M[0]*M[1]);
+  normalize(M[1]);
+}
+
+inline SO2 SO2::exp(const double d){
+  Matrix<2> m;
+  m[0][0] = m[1][1] = cos(d);
+  m[1][0] = sin(d);
+  m[0][1] = -m[1][0];
+  return SO2(m);
+}
+
+inline double SO2::ln() const{
+  return atan2(my_matrix[1][0], my_matrix[0][0]);
+}
+
+inline SO2 SO2::inverse() const{
+    return SO2(*this, Invert());
+}
+
+inline Vector<2> SO2::generator(Vector<2> pos){
+  Vector<2> result;
+  result[0] = pos[1];
+  result[1] = -pos[0];
+  return result;
+}
+ 
+#ifndef TOON_NO_NAMESPACE
+}
+#endif
+
+#endif




reply via email to

[Prev in Thread] Current Thread [Next in Thread]