[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN so2.h so3.h test/SXX_test.cc sim2.h sim3.h
|
From: |
Gerhard Reitmayr |
|
Subject: |
[Toon-members] TooN so2.h so3.h test/SXX_test.cc sim2.h sim3.h |
|
Date: |
Wed, 29 Jun 2011 20:20:01 +0000 |
CVSROOT: /cvsroot/toon
Module name: TooN
Changes by: Gerhard Reitmayr <gerhard> 11/06/29 20:20:00
Modified files:
. : so2.h so3.h
test : SXX_test.cc
Added files:
. : sim2.h sim3.h
Log message:
added 2D and 3D similarity group implementations
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so2.h?cvsroot=toon&r1=1.13&r2=1.14
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.49&r2=1.50
http://cvs.savannah.gnu.org/viewcvs/TooN/sim2.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/TooN/sim3.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/TooN/test/SXX_test.cc?cvsroot=toon&r1=1.10&r2=1.11
Patches:
Index: so2.h
===================================================================
RCS file: /cvsroot/toon/TooN/so2.h,v
retrieving revision 1.13
retrieving revision 1.14
diff -u -b -r1.13 -r1.14
--- so2.h 7 Apr 2011 23:17:02 -0000 1.13
+++ so2.h 29 Jun 2011 20:20:00 -0000 1.14
@@ -36,11 +36,13 @@
namespace TooN {
-template<typename Precision> class SO2;
+template <typename Precision> class SO2;
template <typename Precision> class SE2;
+template <typename Precision> class SIM2;
template<typename Precision> inline std::istream & operator>>(std::istream &,
SO2<Precision> & );
template<typename Precision> inline std::istream & operator>>(std::istream &,
SE2<Precision> & );
+template<typename Precision> inline std::istream & operator>>(std::istream &,
SIM2<Precision> & );
/// Class to represent a two-dimensional rotation matrix. Two-dimensional
rotation
/// matrices are members of the Special Orthogonal Lie group SO2. This group
can be parameterised with
@@ -50,6 +52,7 @@
class SO2 {
friend std::istream& operator>> <Precision>(std::istream&, SO2& );
friend std::istream& operator>> <Precision>(std::istream&,
SE2<Precision>& );
+ friend std::istream& operator>> <Precision>(std::istream&,
SIM2<Precision>& );
public:
/// Default constructor. Initialises the matrix to the identity (no
rotation)
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.49
retrieving revision 1.50
diff -u -b -r1.49 -r1.50
--- so3.h 7 Apr 2011 23:17:02 -0000 1.49
+++ so3.h 29 Jun 2011 20:20:00 -0000 1.50
@@ -38,9 +38,11 @@
template <typename Precision> class SO3;
template <typename Precision> class SE3;
+template <typename Precision> class SIM3;
template<class Precision> inline std::istream & operator>>(std::istream &,
SO3<Precision> & );
template<class Precision> inline std::istream & operator>>(std::istream &,
SE3<Precision> & );
+template<class Precision> inline std::istream & operator>>(std::istream &,
SIM3<Precision> & );
/// Class to represent a three-dimensional rotation matrix. Three-dimensional
rotation
/// matrices are members of the Special Orthogonal Lie group SO3. This group
can be parameterised
@@ -54,7 +56,9 @@
public:
friend std::istream& operator>> <Precision> (std::istream& is,
SO3<Precision> & rhs);
friend std::istream& operator>> <Precision> (std::istream& is,
SE3<Precision> & rhs);
+ friend std::istream& operator>> <Precision> (std::istream& is,
SIM3<Precision> & rhs);
friend class SE3<Precision>;
+ friend class SIM3<Precision>;
/// Default constructor. Initialises the matrix to the identity (no
rotation)
SO3() : my_matrix(Identity) {}
Index: test/SXX_test.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/SXX_test.cc,v
retrieving revision 1.10
retrieving revision 1.11
diff -u -b -r1.10 -r1.11
--- test/SXX_test.cc 1 Feb 2010 17:33:18 -0000 1.10
+++ test/SXX_test.cc 29 Jun 2011 20:20:00 -0000 1.11
@@ -7,6 +7,8 @@
#include <TooN/se2.h>
#include <TooN/so3.h>
#include <TooN/se3.h>
+#include <TooN/sim2.h>
+#include <TooN/sim3.h>
void test_so2(){
cout << "---------------SO2 Tests---------------\n";
@@ -246,6 +248,158 @@
cout << r << endl;
}
+void test_sim3(){
+ cout << "---------------SIM3 Tests---------------\n";
+
+ TooN::SIM3<> r1;
+ cout << "default constructor\n";
+ cout << r1 << endl;
+ cout << "default constructor <int>\n";
+ TooN::SIM3<int> r2;
+ cout << r2 << endl;
+ TooN::SIM3<> r(TooN::makeVector(0.1, 0.1, 0.1, 0.2, -0.2, 0.2, 0.5));
+ cout << "constructor\n";
+ cout << r << endl;
+ cout << "generator 0,1,2,3,4,5,6\n";
+ cout << TooN::SIM3<>::generator(0) ;
+ cout << TooN::SIM3<>::generator(1) ;
+ cout << TooN::SIM3<>::generator(2) << endl;
+ cout << TooN::SIM3<>::generator(3) ;
+ cout << TooN::SIM3<>::generator(4) ;
+ cout << TooN::SIM3<>::generator(5) << endl;
+ cout << TooN::SIM3<>::generator(6) << endl;
+ cout << "ln()\n";
+ cout << r.ln() << endl;
+ cout << "inverse\n";
+ cout << r.inverse() << endl;
+ cout << "inverse.ln()\n";
+ cout << r.inverse().ln() << endl;
+ cout << "times\n";
+ cout << r * r.inverse() << endl;
+ cout << (r *= r.inverse()) << endl;
+ r = TooN::SIM3<>::exp(TooN::makeVector(0.1, 0.1, 0.1, 0.2, -0.2, 0.2,
0.5));
+
+ 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::Zeros);
+ 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;
+
+#if 0
+ 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;
+#endif
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 0 1 1 0 0 2 0 0 1 3");
+ is >> r;
+ cout << r << endl;
+ cout << r.get_rotation() << "\n" << r.get_translation() << "\n" <<
r.get_scale() << endl;
+}
+
+void test_sim2(){
+ cout << "---------------SIM2 Tests---------------\n";
+
+ TooN::SIM2<> r1;
+ cout << "default constructor\n";
+ cout << r1 << endl;
+ cout << "default constructor <int>\n";
+ TooN::SIM2<int> r2;
+ cout << r2 << endl;
+ TooN::SIM2<> r(TooN::makeVector(0.1, 0.1, 0.1, 0.2));
+ cout << "constructor\n";
+ cout << r << endl;
+ cout << "generator 0,1,2,3\n";
+ cout << TooN::SIM2<>::generator(0);
+ cout << TooN::SIM2<>::generator(1);
+ cout << TooN::SIM2<>::generator(2) << endl;
+ cout << TooN::SIM2<>::generator(3) << endl;
+ cout << "ln()\n";
+ cout << r.ln() << endl;
+ cout << "inverse\n";
+ cout << r.inverse() << endl;
+ cout << "inverse.ln()\n";
+ cout << r.inverse().ln() << endl;
+ cout << "times\n";
+ cout << r * r.inverse() << endl;
+ cout << (r *= r.inverse()) << endl;
+ r = TooN::SIM2<>::exp(TooN::makeVector(0.1, 0.1, 0.1, 0.2));
+
+ TooN::Vector<3> t = TooN::makeVector(0,1,2);
+ cout << "right and left multiply with vector " << t << "\n";
+ cout << r * t << endl;
+ cout << t * r << endl;
+ TooN::Vector<2> t3 = TooN::makeVector(0,1);
+ cout << "right with a 2 vector " << t3 << "\n";
+ cout << r * t3 << endl;
+
+ TooN::Matrix<3> l = TooN::Identity;
+ cout << "right and left multiply with matrix\n" << l << "\n";
+ cout << r * l << endl;
+ cout << l * r << endl;
+ TooN::Matrix<3,6> l2(TooN::Zeros);
+ l2[0] = TooN::makeVector(0,1,2,3,4,5);
+ cout << "right with rectangular matrix\n" << l2 << "\n";
+ cout << r * l2 << endl;
+ cout << l2.T() * r << endl;
+
+#if 0
+ 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;
+#endif
+
+ cout << "read from istream\n";
+ istringstream is("0 -1 0 1 1 0");
+ is >> r;
+ cout << r << endl;
+ cout << r.get_rotation() << "\n" << r.get_translation() << "\n" <<
r.get_scale() << endl;
+}
void test_se2_exp(){
cout << "------------------SE2 check------------------\n";
@@ -261,12 +415,17 @@
int main(int, char* *){
+ #if 0
test_so2();
test_so3();
test_se2();
test_se3();
+ test_sim3();
test_se2_exp();
+#endif
+
+ test_sim2();
return 0;
}
Index: sim2.h
===================================================================
RCS file: sim2.h
diff -N sim2.h
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ sim2.h 29 Jun 2011 20:20:00 -0000 1.1
@@ -0,0 +1,406 @@
+// -*- c++ -*-
+
+// Copyright (C) 2011 Tom Drummond (address@hidden),
+// Ed Rosten (address@hidden), Gerhard Reitmayr (address@hidden)
+//
+// This file is part of the TooN Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 2, 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 General Public License for more details.
+
+// You should have received a copy of the GNU General Public License along
+// with this library; see the file COPYING. If not, write to the Free
+// Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
+// USA.
+
+// As a special exception, you may use this file as part of a free software
+// library without restriction. Specifically, if other files instantiate
+// templates or use macros or inline functions from this file, or you compile
+// this file and link it with other files to produce an executable, this
+// file does not by itself cause the resulting executable to be covered by
+// the GNU General Public License. This exception does not however
+// invalidate any other reasons why the executable file might be covered by
+// the GNU General Public License.
+
+/* This code mostly made by copying from sim3.h !! */
+
+#ifndef TOON_INCLUDE_SIM2_H
+#define TOON_INCLUDE_SIM2_H
+
+#include <TooN/se2.h>
+
+namespace TooN {
+
+/// Represent a two-dimensional Similarity transformation (a rotation, a
uniform scale and a translation).
+/// This can be represented by a \f$2\times 3\f$ matrix operating on a
homogeneous co-ordinate,
+/// so that a vector \f$\underline{x}\f$ is transformed to a new location
\f$\underline{x}'\f$
+/// by
+/// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\
\begin{bmatrix}x'\\y'\end{bmatrix} &= \begin{pmatrix}r_{11} & r_{12} &
t_1\\r_{21} & r_{22} &
t_2\end{pmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix}\end{aligned}\f]
+///
+/// This transformation is a member of the Lie group SIM2. These can be
parameterised with
+/// four numbers (in the space of the Lie Algebra). In this class, the first
two parameters are a
+/// translation vector while the third is the amount of rotation in the plane
as for SO2. The forth is the logarithm of the scale factor.
+/// @ingroup gTransforms
+template <typename Precision = DefaultPrecision>
+class SIM2 {
+public:
+ /// Default constructor. Initialises the the rotation to zero (the
identity), the scale factor to one and the translation to zero
+ SIM2() : my_scale(1) {}
+ template <class A> SIM2(const SO2<Precision>& R, const
Vector<2,Precision,A>& T, const Precision s) : my_se2(R,T), my_scale(s) {}
+ template <int S, class P, class A> SIM2(const Vector<S, P, A> & v) {
*this = exp(v); }
+
+ /// Returns the rotation part of the transformation as a SO2
+ SO2<Precision> & get_rotation(){return my_se2.get_rotation();}
+ /// @overload
+ const SO2<Precision> & get_rotation() const {return
my_se2.get_rotation();}
+ /// Returns the translation part of the transformation as a Vector
+ Vector<2, Precision> & get_translation() {return
my_se2.get_translation();}
+ /// @overload
+ const Vector<2, Precision> & get_translation() const {return
my_se2.get_translation();}
+
+ /// Returns the scale factor of the transformation
+ Precision & get_scale() {return my_scale;}
+ /// @overload
+ const Precision & get_scale() const {return my_scale;}
+
+ /// Exponentiate a Vector in the Lie Algebra to generate a new SIM2.
+ /// See the Detailed Description for details of this vector.
+ /// @param vect The Vector to exponentiate
+ template <int S, typename P, typename A>
+ static inline SIM2 exp(const Vector<S,P, A>& vect);
+
+ /// Take the logarithm of the matrix, generating the corresponding
vector in the Lie Algebra.
+ /// See the Detailed Description for details of this vector.
+ static inline Vector<4, Precision> ln(const SIM2& se2);
+ /// @overload
+ Vector<4, Precision> ln() const { return SIM2::ln(*this); }
+
+ /// compute the inverse of the transformation
+ SIM2 inverse() const {
+ const SO2<Precision> & rinv = get_rotation().inverse();
+ const Precision inv_scale = 1/get_scale();
+ return SIM2(rinv, -(rinv*(inv_scale*get_translation())),
inv_scale);
+ };
+
+ /// Right-multiply by another SIM2 (concatenate the two transformations)
+ /// @param rhs The multipier
+ template <typename P>
+ SIM2<typename Internal::MultiplyType<Precision,P>::type> operator
*(const SIM2<P>& rhs) const {
+ return SIM2<typename
Internal::MultiplyType<Precision,P>::type>(get_rotation()*rhs.get_rotation(),
get_translation() + get_rotation() * (get_scale()*rhs.get_translation()),
get_scale() * rhs.get_scale());
+ }
+
+ /// Self right-multiply by another SIM2 (concatenate the two
transformations)
+ /// @param rhs The multipier
+ inline SIM2& operator *=(const SIM2& rhs) {
+ *this = *this * rhs;
+ return *this;
+ }
+
+ /// returns the generators for the Lie group. These are a set of
matrices that
+ /// form a basis for the vector space of the Lie algebra.
+ /// - 0 is translation in x
+ /// - 1 is translation in y
+ /// - 2 is rotation in the plane
+ /// - 3 is uniform scale
+ static inline Matrix<3,3, Precision> generator(int i) {
+ Matrix<3,3,Precision> result(Zeros);
+ switch(i){
+ case 0:
+ case 1:
+ result(i,2) = 1;
+ break;
+ case 2:
+ result(0,1) = -1;
+ result(1,0) = 1;
+ break;
+ case 3:
+ result(0,0) = 1;
+ result(1,1) = 1;
+ break;
+ }
+ return result;
+ }
+
+ /// 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>
+ Vector<4, Precision> adjoint(const Vector<4,Precision, Accessor> &
vect) const {
+ Vector<4, Precision> result;
+ result[2] = vect[2];
+ result.template slice<0,2>() = get_rotation() * vect.template
slice<0,2>();
+ result[0] += vect[2] * get_translation()[1];
+ result[1] -= vect[2] * get_translation()[0];
+ return result;
+ }
+
+ template <typename Accessor>
+ Matrix<4,4,Precision> adjoint(const Matrix<4,4,Precision,Accessor>& M)
const {
+ Matrix<4,4,Precision> result;
+ for(int i=0; i<4; ++i)
+ result.T()[i] = adjoint(M.T()[i]);
+ for(int i=0; i<4; ++i)
+ result[i] = adjoint(result[i]);
+ return result;
+ }
+
+private:
+ SE2<Precision> my_se2;
+ Precision my_scale;
+};
+
+/// Write an SIM2 to a stream
+/// @relates SIM2
+template <class Precision>
+inline std::ostream& operator<<(std::ostream& os, const SIM2<Precision> & rhs){
+ std::streamsize fw = os.width();
+ for(int i=0; i<2; i++){
+ os.width(fw);
+ os << rhs.get_rotation().get_matrix()[i] * rhs.get_scale();
+ os.width(fw);
+ os << rhs.get_translation()[i] << '\n';
+ }
+ return os;
+}
+
+/// Read an SIM2 from a stream
+/// @relates SIM2
+template <class Precision>
+inline std::istream& operator>>(std::istream& is, SIM2<Precision>& rhs){
+ for(int i=0; i<2; i++)
+ is >> rhs.get_rotation().my_matrix[i].ref() >>
rhs.get_translation()[i];
+ rhs.get_scale() = (norm(rhs.get_rotation().my_matrix[0]) +
norm(rhs.get_rotation().my_matrix[1]))/2;
+ rhs.get_rotation().coerce();
+ return is;
+}
+
+
+//////////////////
+// operator * //
+// SIM2 * Vector //
+//////////////////
+
+namespace Internal {
+template<int S, typename P, typename PV, typename A>
+struct SIM2VMult;
+}
+
+template<int S, typename P, typename PV, typename A>
+struct Operator<Internal::SIM2VMult<S,P,PV,A> > {
+ const SIM2<P> & lhs;
+ const Vector<S,PV,A> & rhs;
+
+ Operator(const SIM2<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()*(lhs.get_scale()*rhs.template slice<0,2>());
+ res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
+ res[2] = rhs[2];
+ }
+ int size() const { return 3; }
+};
+
+/// Right-multiply with a Vector<3>
+/// @relates SIM2
+template<int S, typename P, typename PV, typename A>
+inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const
SIM2<P> & lhs, const Vector<S,PV,A>& rhs){
+ return Vector<3, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::SIM2VMult<S,P,PV,A>
>(lhs,rhs));
+}
+
+/// Right-multiply with a Vector<2> (special case, extended to be a
homogeneous vector)
+/// @relates SIM2
+template <typename P, typename PV, typename A>
+inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const
SIM2<P>& lhs, const Vector<2,PV,A>& rhs){
+ return lhs.get_translation() + lhs.get_rotation() * (lhs.get_scale() *
rhs);
+}
+
+//////////////////
+// operator * //
+// Vector * SIM2 //
+//////////////////
+
+namespace Internal {
+template<int S, typename P, typename PV, typename A>
+struct VSIM2Mult;
+}
+
+template<int S, typename P, typename PV, typename A>
+struct Operator<Internal::VSIM2Mult<S,P,PV,A> > {
+ const Vector<S,PV,A> & lhs;
+ const SIM2<P> & rhs;
+
+ Operator(const Vector<S,PV,A> & l, const SIM2<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_scale())*rhs.get_rotation();
+ res[2] = lhs[2];
+ res[2] += lhs.template slice<0,2>() * rhs.get_translation();
+ }
+ int size() const { return 3; }
+};
+
+/// Left-multiply with a Vector<3>
+/// @relates SIM2
+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 SIM2<P> & rhs){
+ return Vector<3, typename
Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSIM2Mult<S, P,PV,A>
>(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// SIM2 * Matrix //
+//////////////////
+
+namespace Internal {
+template <int R, int C, typename PM, typename A, typename P>
+struct SIM2MMult;
+}
+
+template<int R, int Cols, typename PM, typename A, typename P>
+struct Operator<Internal::SIM2MMult<R, Cols, PM, A, P> > {
+ const SIM2<P> & lhs;
+ const Matrix<R,Cols,PM,A> & rhs;
+
+ Operator(const SIM2<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; }
+};
+
+/// Right-multiply with a Matrix<3>
+/// @relates SIM2
+template <int R, int Cols, typename PM, typename A, typename P>
+inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type>
operator*(const SIM2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
+ return Matrix<3,Cols,typename
Internal::MultiplyType<P,PM>::type>(Operator<Internal::SIM2MMult<R, Cols, PM,
A, P> >(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// Matrix * SIM2 //
+//////////////////
+
+namespace Internal {
+template <int Rows, int C, typename PM, typename A, typename P>
+struct MSIM2Mult;
+}
+
+template<int Rows, int C, typename PM, typename A, typename P>
+struct Operator<Internal::MSIM2Mult<Rows, C, PM, A, P> > {
+ const Matrix<Rows,C,PM,A> & lhs;
+ const SIM2<P> & rhs;
+
+ Operator( const Matrix<Rows,C,PM,A> & l, const SIM2<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(); }
+};
+
+/// Left-multiply with a Matrix<3>
+/// @relates SIM2
+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 SIM2<P> & rhs ){
+ return Matrix<Rows,3,typename
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSIM2Mult<Rows, C, PM,
A, P> >(lhs,rhs));
+}
+
+template <typename Precision>
+template <int S, typename PV, typename Accessor>
+inline SIM2<Precision> SIM2<Precision>::exp(const Vector<S, PV, Accessor>& mu){
+ SizeMismatch<4,S>::test(4, mu.size());
+
+ static const Precision one_6th = 1.0/6.0;
+ static const Precision one_20th = 1.0/20.0;
+
+ using std::exp;
+
+ SIM2<Precision> result;
+
+ // rotation
+ const Precision theta = mu[2];
+ result.get_rotation() = SO2<Precision>::exp(theta);
+
+ // scale
+ const Precision s = mu[3];
+ const Precision es = exp(s);
+ result.get_scale() = es;
+
+ // translation
+ const Precision a = es * sin(theta);
+ const Precision b = es * cos(theta);
+ const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
+
+ const Precision inv_s_theta = 1/(s*s + theta * theta);
+
+ const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
+ const Precision CCC = ((b-1)*s + a*theta) * inv_s_theta;
+
+ const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta *
mu[0]);
+ result.get_translation() = CCC * mu.template slice<0,2>() +
TooN::operator*(CC, cross);
+
+ return result;
+}
+
+template <typename Precision>
+inline Vector<4, Precision> SIM2<Precision>::ln(const SIM2<Precision> & sim2) {
+ using std::log;
+
+ Vector<4, Precision> result;
+
+ // rotation
+ const Precision theta = sim2.get_rotation().ln();
+ result[2] = theta;
+
+ // scale
+ const Precision es = sim2.get_scale();
+ const Precision s = log(sim2.get_scale());
+ result[3] = s;
+
+ // translation
+ const Precision a = es * sin(theta);
+ const Precision b = es * cos(theta);
+ const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
+
+ const Precision inv_s_theta = 1/(s*s + theta * theta);
+
+ const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
+ const Precision CCC = ((b-1)*s + a*theta) * inv_s_theta;
+
+ Matrix<2,2, Precision> cross = Zeros; cross(0,1) = -theta; cross(1,0) =
theta;
+ const Matrix<2,2, Precision> W = CCC * Identity + CC * cross;
+ result.template slice<0,2>() = gaussian_elimination(W,
sim2.get_translation());
+
+ return result;
+}
+
+#if 0
+/// Multiply a SO2 with and SE2
+/// @relates SE2
+/// @relates SO2
+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());
+}
+#endif
+
+}
+#endif
Index: sim3.h
===================================================================
RCS file: sim3.h
diff -N sim3.h
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ sim3.h 29 Jun 2011 20:20:00 -0000 1.1
@@ -0,0 +1,503 @@
+// -*- c++ -*-
+
+// Copyright (C) 2011 Tom Drummond (address@hidden)
+//
+// This file is part of the TooN Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 2, 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 General Public License for more details.
+
+// You should have received a copy of the GNU General Public License along
+// with this library; see the file COPYING. If not, write to the Free
+// Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
+// USA.
+
+// As a special exception, you may use this file as part of a free software
+// library without restriction. Specifically, if other files instantiate
+// templates or use macros or inline functions from this file, or you compile
+// this file and link it with other files to produce an executable, this
+// file does not by itself cause the resulting executable to be covered by
+// the GNU General Public License. This exception does not however
+// invalidate any other reasons why the executable file might be covered by
+// the GNU General Public License.
+
+#ifndef TOON_INCLUDE_SIM3_H
+#define TOON_INCLUDE_SIM3_H
+
+#include <TooN/se3.h>
+
+namespace TooN {
+
+
+/// Represent a three-dimensional similarity transformation (a rotation, a
scale factor and a translation).
+/// This can be represented by a \f$3\times\f$4 matrix operating on a
homogeneous co-ordinate,
+/// so that a vector \f$\underline{x}\f$ is transformed to a new location
\f$\underline{x}'\f$
+/// by
+/// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\
\begin{bmatrix}x'\\y'\\z'\end{bmatrix} &= \begin{pmatrix}r_{11} & r_{12} &
r_{13} & t_1\\r_{21} & r_{22} & r_{23} & t_2\\r_{31} & r_{32} & r_{33} &
t_3\end{pmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\end{aligned}\f]
+///
+/// This transformation is a member of the Lie group SIM3. These can be
parameterised with
+/// seven numbers (in the space of the Lie Algebra). In this class, the first
three parameters are a
+/// translation vector while the second three are a rotation vector, whose
direction is the axis of rotation
+/// and length the amount of rotation (in radians), as for SO3. The seventh
parameter is the log of the scale of the transformation.
+/// @ingroup gTransforms
+template <typename Precision = DefaultPrecision>
+class SIM3 {
+public:
+ /// Default constructor. Initialises the the rotation to zero (the
identity), the scale to one and the translation to zero
+ inline SIM3() : my_scale(1) {}
+
+ template <int S, typename P, typename A>
+ SIM3(const SO3<Precision> & R, const Vector<S, P, A>& T, const
Precision & s) : my_se3(R,T), my_scale(s) {}
+ template <int S, typename P, typename A>
+ SIM3(const Vector<S, P, A> & v) { *this = exp(v); }
+
+ /// Returns the rotation part of the transformation as a SO3
+ inline SO3<Precision>& get_rotation(){return my_se3.get_rotation();}
+ /// @overload
+ inline const SO3<Precision>& get_rotation() const {return
my_se3.get_rotation();}
+
+ /// Returns the translation part of the transformation as a Vector
+ inline Vector<3, Precision>& get_translation() {return
my_se3.get_translation();}
+ /// @overload
+ inline const Vector<3, Precision>& get_translation() const {return
my_se3.get_translation();}
+
+ /// Returns the scale factor
+ inline Precision & get_scale() { return my_scale; }
+ /// @overload
+ inline const Precision & get_scale() const { return my_scale; }
+
+ /// Exponentiate a Vector in the Lie Algebra to generate a new SIM3.
+ /// See the Detailed Description for details of this vector.
+ /// @param vect The Vector to exponentiate
+ template <int S, typename P, typename A>
+ static inline SIM3 exp(const Vector<S, P, A>& vect);
+
+ /// Take the logarithm of the matrix, generating the corresponding
vector in the Lie Algebra.
+ /// See the Detailed Description for details of this vector.
+ static inline Vector<7, Precision> ln(const SIM3& se3);
+ /// @overload
+ inline Vector<7, Precision> ln() const { return SIM3::ln(*this); }
+
+ inline SIM3 inverse() const {
+ const SO3<Precision> rinv = get_rotation().inverse();
+ const Precision inv_scale = 1/my_scale;
+ return SIM3(rinv, -(inv_scale*(rinv*get_translation())),
inv_scale);
+ }
+
+ /// Right-multiply by another SIM3 (concatenate the two transformations)
+ /// @param rhs The multipier
+ inline SIM3& operator *=(const SIM3& rhs) {
+ get_translation() += get_rotation() * (get_scale() *
rhs.get_translation());
+ get_rotation() *= rhs.get_rotation();
+ get_scale() *= rhs.get_scale();
+ return *this;
+ }
+
+ /// Right-multiply by another SIM3 (concatenate the two transformations)
+ /// @param rhs The multipier
+ template<typename P>
+ inline SIM3<typename Internal::MultiplyType<Precision, P>::type>
operator *(const SIM3<P>& rhs) const {
+ return SIM3<typename Internal::MultiplyType<Precision,
P>::type>(get_rotation()*rhs.get_rotation(), get_translation() +
get_rotation()*(get_scale()*rhs.get_translation()),
get_scale()*rhs.get_scale());
+ }
+
+ inline SIM3& left_multiply_by(const SIM3& left) {
+ get_translation() = left.get_translation() +
left.get_rotation() * (left.get_scale() * get_translation());
+ get_rotation() = left.get_rotation() * get_rotation();
+ get_scale() = left.get_scale() * get_scale();
+ return *this;
+ }
+
+ static inline Matrix<4,4,Precision> generator(int i){
+ Matrix<4,4,Precision> result(Zeros);
+ if(i < 3){
+ result(i,3)=1;
+ return result;
+ }
+ if(i < 6){
+ result[(i+1)%3][(i+2)%3] = -1;
+ result[(i+2)%3][(i+1)%3] = 1;
+ return result;
+ }
+ result(0,0) = 1;
+ result(1,1) = 1;
+ result(2,2) = 1;
+ return result;
+ }
+
+ /// Returns the i-th generator times pos
+ template<typename Base>
+ inline static Vector<4,Precision> generator_field(int i, const
Vector<4, Precision, Base>& pos)
+ {
+ Vector<4, Precision> result(Zeros);
+ if(i < 3){
+ result[i]=pos[3];
+ return result;
+ }
+ if(i < 6){
+ result[(i+1)%3] = - pos[(i+2)%3];
+ result[(i+2)%3] = pos[(i+1)%3];
+ return result;
+ }
+ result.template slice<0,3>() = pos.template slice<0,3>();
+ return result;
+ }
+
+ /// Transfer a matrix in the Lie Algebra from one
+ /// co-ordinate frame to another. This is the operation such that for a
matrix
+ /// \f$ B \f$,
+ /// \f$ e^{\text{Adj}(v)} = Be^{v}B^{-1} \f$
+ /// @param M The Matrix to transfer
+ template<int S, typename P2, typename Accessor>
+ inline Vector<7, Precision> adjoint(const Vector<S,P2, Accessor>&
vect)const;
+
+ /// Transfer covectors between frames (using the transpose of the
inverse of the adjoint)
+ /// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
+ template<int S, typename P2, typename Accessor>
+ inline Vector<7, Precision> trinvadjoint(const Vector<S,P2,Accessor>&
vect)const;
+
+ ///@overload
+ template <int R, int C, typename P2, typename Accessor>
+ inline Matrix<7,7,Precision> adjoint(const Matrix<R,C,P2,Accessor>&
M)const;
+
+ ///@overload
+ template <int R, int C, typename P2, typename Accessor>
+ inline Matrix<7,7,Precision> trinvadjoint(const
Matrix<R,C,P2,Accessor>& M)const;
+
+private:
+ SE3<Precision> my_se3;
+ Precision my_scale;
+};
+
+// 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 P2, typename Accessor>
+inline Vector<7, Precision> SIM3<Precision>::adjoint(const Vector<S,P2,
Accessor>& vect) const{
+ SizeMismatch<7,S>::test(7, vect.size());
+ Vector<7, 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;
+}
+
+// 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 P2, typename Accessor>
+inline Vector<7, Precision> SIM3<Precision>::trinvadjoint(const Vector<S,P2,
Accessor>& vect) const{
+ SizeMismatch<7,S>::test(7, vect.size());
+ Vector<7, 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<typename Precision>
+template<int R, int C, typename P2, typename Accessor>
+inline Matrix<7,7,Precision> SIM3<Precision>::adjoint(const
Matrix<R,C,P2,Accessor>& M)const{
+ SizeMismatch<7,R>::test(7, M.num_cols());
+ SizeMismatch<7,C>::test(7, M.num_rows());
+
+ Matrix<7,7,Precision> result;
+ for(int i=0; i<7; i++){
+ result.T()[i] = adjoint(M.T()[i]);
+ }
+ for(int i=0; i<7; i++){
+ result[i] = adjoint(result[i]);
+ }
+ return result;
+}
+
+template<typename Precision>
+template<int R, int C, typename P2, typename Accessor>
+inline Matrix<7,7,Precision> SIM3<Precision>::trinvadjoint(const
Matrix<R,C,P2,Accessor>& M)const{
+ SizeMismatch<7,R>::test(7, M.num_cols());
+ SizeMismatch<7,C>::test(7, M.num_rows());
+
+ Matrix<7,7,Precision> result;
+ for(int i=0; i<7; i++){
+ result.T()[i] = trinvadjoint(M.T()[i]);
+ }
+ for(int i=0; i<7; i++){
+ result[i] = trinvadjoint(result[i]);
+ }
+ return result;
+}
+
+/// Write an SIM3 to a stream
+/// @relates SIM3
+template <typename Precision>
+inline std::ostream& operator <<(std::ostream& os, const SIM3<Precision>& rhs){
+ std::streamsize fw = os.width();
+ for(int i=0; i<3; i++){
+ os.width(fw);
+ os << rhs.get_rotation().get_matrix()[i] * rhs.get_scale();
+ os.width(fw);
+ os << rhs.get_translation()[i] << '\n';
+ }
+ return os;
+}
+
+
+/// Reads an SIM3 from a stream
+/// @relates SIM3
+template <typename Precision>
+inline std::istream& operator>>(std::istream& is, SIM3<Precision>& rhs){
+ for(int i=0; i<3; i++){
+ is >> rhs.get_rotation().my_matrix[i].ref() >>
rhs.get_translation()[i];
+ }
+ rhs.get_scale() = (norm(rhs.get_rotation().my_matrix[0]) +
norm(rhs.get_rotation().my_matrix[1]) +
norm(rhs.get_rotation().my_matrix[2]))/3;
+ rhs.get_rotation().coerce();
+ return is;
+}
+
+//////////////////
+// operator * //
+// SIM3 * Vector //
+//////////////////
+
+namespace Internal {
+template<int S, typename PV, typename A, typename P>
+struct SIM3VMult;
+}
+
+template<int S, typename PV, typename A, typename P>
+struct Operator<Internal::SIM3VMult<S,PV,A,P> > {
+ const SIM3<P> & lhs;
+ const Vector<S,PV,A> & rhs;
+
+ Operator(const SIM3<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() * (lhs.get_scale()
* rhs.template slice<0,3>());
+ res.template
slice<0,3>()+=TooN::operator*(lhs.get_translation(),rhs[3]);
+ res[3] = rhs[3];
+ }
+ int size() const { return 4; }
+};
+
+/// Right-multiply by a Vector
+/// @relates SIM3
+template<int S, typename PV, typename A, typename P> inline
+Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SIM3<P>
& lhs, const Vector<S,PV,A>& rhs){
+ return Vector<4, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::SIM3VMult<S,PV,A,P>
>(lhs,rhs));
+}
+
+/// Right-multiply by a Vector
+/// @relates SIM3
+template <typename PV, typename A, typename P> inline
+Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const
SIM3<P>& lhs, const Vector<3,PV,A>& rhs){
+ return lhs.get_translation() + lhs.get_rotation() * (lhs.get_scale() *
rhs);
+}
+
+//////////////////
+// operator * //
+// Vector * SIM3 //
+//////////////////
+
+namespace Internal {
+template<int S, typename PV, typename A, typename P>
+struct VSIM3Mult;
+}
+
+template<int S, typename PV, typename A, typename P>
+struct Operator<Internal::VSIM3Mult<S,PV,A,P> > {
+ const Vector<S,PV,A> & lhs;
+ const SIM3<P> & rhs;
+
+ Operator( const Vector<S,PV,A> & l, const SIM3<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>()= rhs.get_scale() * 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; }
+};
+
+/// Left-multiply by a Vector
+/// @relates SIM3
+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 SIM3<P> & rhs){
+ return Vector<4, typename
Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSIM3Mult<S,PV,A,P>
>(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// SIM3 * Matrix //
+//////////////////
+
+namespace Internal {
+template <int R, int C, typename PM, typename A, typename P>
+struct SIM3MMult;
+}
+
+template<int R, int Cols, typename PM, typename A, typename P>
+struct Operator<Internal::SIM3MMult<R, Cols, PM, A, P> > {
+ const SIM3<P> & lhs;
+ const Matrix<R,Cols,PM,A> & rhs;
+
+ Operator(const SIM3<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; }
+};
+
+/// Right-multiply by a Matrix
+/// @relates SIM3
+template <int R, int Cols, typename PM, typename A, typename P> inline
+Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const
SIM3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
+ return Matrix<4,Cols,typename
Internal::MultiplyType<P,PM>::type>(Operator<Internal::SIM3MMult<R, Cols, PM,
A, P> >(lhs,rhs));
+}
+
+//////////////////
+// operator * //
+// Matrix * SIM3 //
+//////////////////
+
+namespace Internal {
+template <int Rows, int C, typename PM, typename A, typename P>
+struct MSIM3Mult;
+}
+
+template<int Rows, int C, typename PM, typename A, typename P>
+struct Operator<Internal::MSIM3Mult<Rows, C, PM, A, P> > {
+ const Matrix<Rows,C,PM,A> & lhs;
+ const SIM3<P> & rhs;
+
+ Operator( const Matrix<Rows,C,PM,A> & l, const SIM3<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(); }
+};
+
+/// Left-multiply by a Matrix
+/// @relates SIM3
+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 SIM3<P> & rhs ){
+ return Matrix<Rows,4,typename
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSIM3Mult<Rows, C, PM,
A, P> >(lhs,rhs));
+}
+
+template <typename Precision>
+template <int S, typename P, typename VA>
+inline SIM3<Precision> SIM3<Precision>::exp(const Vector<S, P, VA>& mu){
+ SizeMismatch<7,S>::test(7, mu.size());
+ static const Precision one_6th = 1.0/6.0;
+ static const Precision one_20th = 1.0/20.0;
+
+ using std::sqrt;
+ using std::exp;
+
+ SIM3<Precision> result;
+
+ const Vector<3,Precision> w = mu.template slice<3,3>();
+ const Precision theta_sq = w*w;
+ const Precision theta = sqrt(theta_sq);
+
+ // scale
+ const Precision s = mu[6];
+ const Precision es = exp(s);
+ result.get_scale() = es;
+
+ // rotation
+ Precision A, B;
+ if (theta_sq < 1e-8) {
+ A = 1.0 - one_6th * theta_sq;
+ B = 0.5;
+ } else {
+ 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 Precision inv_theta = 1.0/theta;
+ A = sin(theta) * inv_theta;
+ B = (1 - cos(theta)) * (inv_theta * inv_theta);
+ }
+ }
+ rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
+
+ // translation
+ const Precision a = es * sin(theta);
+ const Precision b = es * cos(theta);
+ const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
+
+ const Precision inv_s_theta = 1/(s*s + theta_sq);
+
+ const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
+ const Precision CCC = (c - ((b-1)*s + a*theta) * inv_s_theta) / (theta
* theta);
+
+ const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
+ result.get_translation() = c * mu.template slice<0,3>() +
TooN::operator*(CC, cross) + TooN::operator*(CCC, (w ^ cross));
+
+ return result;
+}
+
+template <typename Precision>
+inline Vector<7, Precision> SIM3<Precision>::ln(const SIM3<Precision>& sim3) {
+ using std::sqrt;
+ using std::log;
+
+ Vector<7, Precision> result;
+
+ // rotation
+ result.template slice<3,3>() = sim3.get_rotation().ln();
+ const Precision theta_sq = norm_sq(result.template slice<3,3>());
+ const Precision theta = sqrt(theta_sq);
+
+ // scale
+ const Precision es = sim3.get_scale();
+ const Precision s = log(sim3.get_scale());
+ result[6] = s;
+
+ // Translation
+ const Precision a = es * sin(theta);
+ const Precision b = es * cos(theta);
+ const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
+ const Precision inv_s_theta = 1/(s*s + theta_sq);
+
+ const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
+ const Precision CCC = (c - ((b-1)*s + a*theta) * inv_s_theta) / (theta
* theta);
+ const Matrix<3,3, Precision> cross =
cross_product_matrix(result.template slice<3,3>());
+ const Matrix<3,3, Precision> W = Identity * c + cross * CC + cross *
cross * CCC;
+
+ result.template slice<0,3>() = gaussian_elimination(W,
sim3.get_translation());
+
+ return result;
+}
+
+#if 0
+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());
+}
+#endif
+
+}
+
+#endif
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] TooN so2.h so3.h test/SXX_test.cc sim2.h sim3.h,
Gerhard Reitmayr <=