[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag tag/absorient.h src/absorient.cpp test/sim.cpp
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag tag/absorient.h src/absorient.cpp test/sim.cpp |
Date: |
Sat, 25 Jun 2011 09:12:21 +0000 |
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;
+}
- [Toon-members] tag tag/absorient.h src/absorient.cpp test/sim.cpp,
Gerhard Reitmayr <=