[Toon-members] tag tag/absorient.h src/absorient.cpp test/sim.cpp

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      11/06/25 09:12:21

Modified files:
        tag            : absorient.h 
        src            : absorient.cpp 
Added files:
        test           : sim.cpp 

Log message:
        added estimating a similarity transform (se3 + scale) between two 3D 
point sets after Horn

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/absorient.h?cvsroot=toon&r1=1.4&r2=1.5
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.9&r2=1.10
http://cvs.savannah.gnu.org/viewcvs/tag/test/sim.cpp?cvsroot=toon&rev=1.1

Patches:
Index: tag/absorient.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/absorient.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- tag/absorient.h     20 Apr 2009 10:02:35 -0000      1.4
+++ tag/absorient.h     25 Jun 2011 09:12:21 -0000      1.5
@@ -39,6 +39,14 @@
 /// @ingroup absorient
 TooN::SE3<> computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > & 
a, const std::vector<TooN::Vector<3> > & b);
 
+/// computes a similarity transformation between two corresponding point sets 
after Horn
+/// The result is an SE3 and a scale S that maps points from vector a to 
points from vector b : b[i] = SE3 * S * a[i]
+/// @param[in] a vector of 3D points
+/// @param[in] b vector of 3D points
+/// @return a pair consisting of a TooN::SE3 T and a double S containing the 
transformation such that b = T * S * a
+/// @ingroup absorient
+std::pair<TooN::SE3<>, double> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b);
+
 /// computes the mean rotation of a set of rotations. This is the rotation R 
such that R^{-1} * R_i is minimal for all R_i.
 /// @param[in] r a vector of rotations
 /// @return TooN::SO3 mean rotation of all input rotations

Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.9
retrieving revision 1.10
diff -u -b -r1.9 -r1.10
--- src/absorient.cpp   8 May 2009 12:08:11 -0000       1.9
+++ src/absorient.cpp   25 Jun 2011 09:12:21 -0000      1.10
@@ -81,7 +81,7 @@
 }
 
 TooN::SE3<>  computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > & 
a, const std::vector<TooN::Vector<3> > & b){
-    // std::assert(a.size() <= b.size());
+       assert(a.size() <= b.size());
     const size_t N = a.size();
 
     TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
@@ -108,6 +108,44 @@
     return result;
 }
 
+std::pair<TooN::SE3<>, double> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
+       assert(a.size() <= b.size());
+       const size_t N = a.size();
+       
+       TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
+       
+       // compute centroids
+       for(unsigned int i = 0; i < N; ++i){
+               ma += a[i];
+               mb += b[i];
+       }
+       ma /= N;
+       mb /= N;
+       
+       // compute shifted locations
+       std::vector<TooN::Vector<3> > ap(N), bp(N);
+       for( unsigned int i = 0; i < N; ++i){
+               ap[i] = a[i] - ma;
+               bp[i] = b[i] - ma;
+       }
+       
+       // put resulting transformation together 
+       TooN::SE3<>  result;
+       result.get_rotation() = computeOrientation( ap, bp );
+       
+       // compute scale
+       double sa = 0, sbRa = 0;
+       for( unsigned int i = 0; i < N; ++i){
+               sa += norm_sq(ap[i]);
+               sbRa += bp[i] * (result.get_rotation() * ap[i]);
+       }
+       
+       const double scale = sbRa/sa;
+       
+       result.get_translation() = mb - result.get_rotation() * (scale * ma);
+       return std::make_pair(result, scale);
+}
+
 TooN::SO3<>  computeMeanOrientation( const std::vector<TooN::SO3<> > & r){
     const size_t N = r.size();
     std::vector<TooN::SO3<> > rt(N);

Index: test/sim.cpp
===================================================================
RCS file: test/sim.cpp
diff -N test/sim.cpp
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ test/sim.cpp        25 Jun 2011 09:12:21 -0000      1.1
@@ -0,0 +1,57 @@
+#include <iostream>
+#include <vector>
+
+#include <tag/absorient.h>
+
+using namespace std;
+using namespace TooN;
+using namespace tag;
+
+typedef vector<Vector<3> > Points;
+typedef pair<SE3<>, double> Sim;
+
+double inline rand_u(){
+    return (double)rand()/RAND_MAX;
+}
+
+double inline rand_u(double from, double to){
+    return from + (to - from)*rand_u();
+}
+
+template <int N>
+Vector<N> rand_vect(double from, double to){
+    Vector<N> v;
+    for(int i = 0; i < N; ++i)
+        v[i] = rand_u(from, to);
+    return v;
+}
+
+int main( int arg, char ** argv ){
+
+    const int N = arg > 1 ? atoi(argv[1]) : 10;
+
+    Points a(N), b(N), c(N);
+    
+    SE3<> pose(makeVector(1,2,3,2,1,0.5));
+    const double scale = 0.37;
+    
+    for( unsigned i = 0; i < N; ++i ){
+        a[i] = rand_vect<3>(-10, 10);
+        b[i] = pose * (scale * a[i]);
+        c[i] = b[i] + rand_vect<3>(-0.01, 0.01);
+    }
+
+    Sim sb = computeSimilarity(a,b);
+    Sim sc = computeSimilarity(a,c);
+
+    double eb = 0, ec = 0;
+    for( unsigned i = 0; i < N; ++i){
+        eb += norm_sq(b[i] - sb.first * (sb.second * a[i]));
+        ec += norm_sq(c[i] - sc.first * (sc.second * a[i]));
+    }
+
+    cout << sb.first.ln() << "\t" << sb.second << "\t" << sqrt( eb ) << "\n"
+         << sc.first.ln() << "\t" << sc.second << "\t" << sqrt( ec ) << endl;
+
+    return 0;
+}