[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag src/absorient.cpp tag/absorient.h test/abso...
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag src/absorient.cpp tag/absorient.h test/abso... |
Date: |
Mon, 27 Jun 2011 14:56:25 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 11/06/27 14:56:24
Modified files:
src : absorient.cpp
tag : absorient.h
Added files:
test : absorient.cpp
Log message:
updated absolute orientation functions with a different method to
calculate the rotation and made them more robust against input with too little
constraints as well
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.10&r2=1.11
http://cvs.savannah.gnu.org/viewcvs/tag/tag/absorient.h?cvsroot=toon&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/tag/test/absorient.cpp?cvsroot=toon&rev=1.1
Patches:
Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.10
retrieving revision 1.11
diff -u -b -r1.10 -r1.11
--- src/absorient.cpp 25 Jun 2011 09:12:21 -0000 1.10
+++ src/absorient.cpp 27 Jun 2011 14:56:24 -0000 1.11
@@ -2,11 +2,14 @@
#include <cassert>
-#include <TooN/SymEigen.h>
+#include <TooN/svd.h>
#include <TooN/helpers.h>
+#include <TooN/determinant.h>
namespace tag {
+static const TooN::DefaultPrecision eps = 1e-8;
+
TooN::Matrix<3> quaternionToMatrix( const TooN::Vector<4> & q ){
TooN::Matrix<3> result;
const int w = 0, x = 1, y = 2, z = 3;
@@ -22,39 +25,42 @@
return result;
}
-TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > & a,
const std::vector<TooN::Vector<3> > & b ){
+static std::pair<TooN::SO3<>, TooN::DefaultPrecision> computeOrientationScale(
const std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > &
b ){
const size_t N = a.size();
// compute cross correlations
- const int x = 0, y = 1, z = 2;
TooN::Matrix<3> s = TooN::Zeros;
- for( unsigned int i = 0; i < N; i++){
- s += a[i].as_col() * b[i].as_row();
+ for( size_t i = 0; i < N; i++){
+ s += b[i].as_col() * a[i].as_row();
}
+ s /= N;
- // create symmetric M for eigenvalue analysis
- TooN::Matrix<4> M;
- M[0][0] = s[x][x] + s[y][y] + s[z][z];
- M[1][0] = M[0][1] = s[y][z] - s[z][y];
- M[2][0] = M[0][2] = s[z][x] - s[x][z];
- M[3][0] = M[0][3] = s[x][y] - s[y][x];
- M[1][1] = s[x][x] - s[y][y] - s[z][z];
- M[2][1] = M[1][2] = s[x][y] + s[y][x];
- M[3][1] = M[1][3] = s[z][x] + s[x][z];
- M[2][2] = - s[x][x] + s[y][y] - s[z][z];
- M[3][2] = M[2][3] = s[y][z] + s[z][y];
- M[3][3] = - s[x][x] - s[y][y] + s[z][z];
-
- // eigenvalue decomposition to find eigenvector to largest eigenvalue
- TooN::SymEigen<4> ev(M);
- TooN::Vector<4> evals = ev.get_evalues();
- int index = 0;
- for(unsigned int i = index+1; i<4; i++)
- if( evals[i] > evals[index] )
- index = i;
- TooN::Vector<4> evec = ev.get_evectors()[index];
- TooN::SO3<> result;
- result = quaternionToMatrix(evec);
- return result;
+ // SVD of cross correlation matrix
+ TooN::SVD<3> svd(s);
+
+ // build S for rotation matrix
+ TooN::Matrix<3> S = TooN::Identity;
+
+ const TooN::DefaultPrecision ds = determinant_gaussian_elimination(s);
+ if(ds < -eps){
+ S(2,2) = -1;
+ } else if(ds < eps) { // close to 0 let U * VT decide
+ const TooN::DefaultPrecision duv =
determinant_gaussian_elimination(svd.get_U())
+
* determinant_gaussian_elimination(svd.get_VT());
+ if(duv < 0)
+ S(2,2) = -1;
+ }
+
+ // compute trace(DS)
+ TooN::DefaultPrecision scale = 0;
+ for(int i = 0; i < 3; ++i)
+ scale += svd.get_diagonal()[i] * S(i,i);
+
+ return std::make_pair(TooN::SO3<>(svd.get_U() * S * svd.get_VT()),
scale);
+}
+
+TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > & a, const
std::vector<TooN::Vector<3> > & b ){
+ std::pair<TooN::SO3<>, TooN::DefaultPrecision> result =
computeOrientationScale( a, b );
+ return result.first;
}
// computes the orientation from (e1,e2,e3) -> (a,(a^b)^a,a^b), which means
that b the second vector is in the a, b plane
@@ -84,6 +90,10 @@
assert(a.size() <= b.size());
const size_t N = a.size();
+ if(N == 1){ // quick special case
+ return TooN::SE3<>(TooN::SO3<>(), b[0] - a[0]);
+ }
+
TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
// compute centroids
@@ -108,10 +118,14 @@
return result;
}
-std::pair<TooN::SE3<>, double> computeSimilarity( const
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
+std::pair<TooN::SE3<>, TooN::DefaultPrecision> 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();
+ if(N == 1){ // quick special case
+ return std::make_pair(TooN::SE3<>(TooN::SO3<>(), b[0] - a[0]),
1);
+ }
+
TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
// compute centroids
@@ -131,16 +145,18 @@
// put resulting transformation together
TooN::SE3<> result;
- result.get_rotation() = computeOrientation( ap, bp );
+ std::pair<TooN::SO3<>, TooN::DefaultPrecision> rs =
computeOrientationScale( ap, bp );
+ result.get_rotation() = rs.first;
+
+ std::cout << rs.second << std::endl;
// compute scale
- double sa = 0, sbRa = 0;
+ TooN::DefaultPrecision sa = 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;
+ sa /= N;
+ const TooN::DefaultPrecision scale = rs.second / sa;
result.get_translation() = mb - result.get_rotation() * (scale * ma);
return std::make_pair(result, scale);
Index: tag/absorient.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/absorient.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- tag/absorient.h 25 Jun 2011 09:12:21 -0000 1.5
+++ tag/absorient.h 27 Jun 2011 14:56:24 -0000 1.6
@@ -13,8 +13,11 @@
/// between sets of 3D correspondences.
/// computes the rotation between two sets of points maximizing b * Ta
-/// this function is part of the absolute orientation algorithm after Horn
-/// and is used in @ref computeAbsoluteOrientation function.
+/// this function is part of the absolute orientation algorithm after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
+/// @ref computeAbsoluteOrientation and @ref computeSimilarity functions use
this
+/// function.
/// @param[in] a vector of 3D points
/// @param[in] b vector of 3D points
/// @return TooN::SO3 containing the rotation such that b = T a
@@ -31,7 +34,9 @@
/// @ingroup absorient
TooN::SO3<> computeOrientation( const TooN::Vector<3> & a1, const
TooN::Vector<3> & b1, const TooN::Vector<3> & a2, const TooN::Vector<3> & b2 );
-/// computes the rigid transformation between two corresponding point sets
after Horn
+/// computes the rigid transformation between two corresponding point sets
after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
/// The result is an SE3 that maps points from vector a to points from vector
b : b[i] = SE3 * a[i]
/// @param[in] a vector of 3D points
/// @param[in] b vector of 3D points
@@ -39,13 +44,15 @@
/// @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
+/// computes a similarity transformation between two corresponding point sets
after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
/// 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);
+std::pair<TooN::SE3<>, TooN::DefaultPrecision> 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
@@ -54,8 +61,7 @@
TooN::SO3<> computeMeanOrientation( const std::vector<TooN::SO3<> > & r);
/// computes a rotation matrix corresponding to a unit quaternion. The
quaternion
-/// is in the format (q0,qx,qy,qz) to fit the absolute orientation algorithm.
This
-/// is a helper function for the @ref computeOrientation function.
+/// is in the format (q0,qx,qy,qz).
/// @param[in] q a 4-vector containing the coefficients of a unit quaternion
as (q0,qx,qy,qz)
/// @return a 3x3 rotation matrix corresponding to the quaternion
/// @ingroup absorient
Index: test/absorient.cpp
===================================================================
RCS file: test/absorient.cpp
diff -N test/absorient.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ test/absorient.cpp 27 Jun 2011 14:56:24 -0000 1.1
@@ -0,0 +1,58 @@
+#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));
+
+ for( unsigned i = 0; i < N; ++i ){
+ a[i] = rand_vect<3>(-10, 10);
+ b[i] = pose * a[i];
+ c[i] = b[i] + rand_vect<3>(-1, 1);
+ }
+
+ SE3<> sb = computeAbsoluteOrientation(a,b);
+ SE3<> sc = computeAbsoluteOrientation(a,c);
+
+ double eb = 0, ec = 0;
+ for( unsigned i = 0; i < N; ++i){
+ eb += norm_sq(b[i] - sb * a[i]);
+ ec += norm_sq(c[i] - sc * a[i]);
+ }
+
+
+ cout << "test\t" << pose.ln() << "\n";
+ cout << "exact\t" << sb.ln() << "\t" << sqrt( eb ) << "\n"
+ << "noisy\t" << sc.ln() << "\t" << sqrt( ec ) << endl;
+
+ return 0;
+}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag src/absorient.cpp tag/absorient.h test/abso...,
Gerhard Reitmayr <=