[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: |
Wed, 29 Jun 2011 20:34:45 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 11/06/29 20:34:45
Modified files:
tag : absorient.h
src : absorient.cpp
test : sim.cpp
Log message:
made the N-dim absolute pose algorithm accessible, use new SIM{2,3}
objects from TooN for 2D and 3D
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/absorient.h?cvsroot=toon&r1=1.6&r2=1.7
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.11&r2=1.12
http://cvs.savannah.gnu.org/viewcvs/tag/test/sim.cpp?cvsroot=toon&r1=1.2&r2=1.3
Patches:
Index: tag/absorient.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/absorient.h,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -b -r1.6 -r1.7
--- tag/absorient.h 27 Jun 2011 14:56:24 -0000 1.6
+++ tag/absorient.h 29 Jun 2011 20:34:45 -0000 1.7
@@ -2,27 +2,103 @@
#define TAG_ABSORIENT_H_
#include <vector>
+#include <tr1/tuple>
#include <TooN/TooN.h>
-#include <TooN/se3.h>
+#include <TooN/sim2.h>
+#include <TooN/sim3.h>
+#include <TooN/svd.h>
namespace tag {
/// @defgroup absorient Absolute Orientation
/// contains various functions to calculate rotations and rigid transformations
-/// between sets of 3D correspondences.
+/// between sets of D-dim correspondences. There are special case functions
+/// for 2D and 3D points sets that directly return SO2, SO3, SE2 and SE3
objects.
-/// computes the rotation between two sets of points maximizing b * Ta
-/// this function is part of the absolute orientation algorithm after
+namespace Internal {
+
+template <int D>
+std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> computeOrientationScale(
const std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > &
b ){
+ TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
+ const int DIM = a.front().size();
+ const size_t N = std::min(a.size(), b.size());
+
+ // compute cross correlations
+ TooN::Matrix<D> s(DIM,DIM);
+ s = TooN::Zeros;
+ for( size_t i = 0; i < N; i++){
+ s += b[i].as_col() * a[i].as_row();
+ }
+ s /= N;
+
+ // SVD of cross correlation matrix
+ TooN::SVD<D> svd(s);
+
+ // build S for rotation matrix
+ TooN::Vector<D> S(DIM);
+ S = TooN::Ones;
+
+ const TooN::DefaultPrecision eps = 1e-8;
+ const TooN::DefaultPrecision ds = determinant_gaussian_elimination(s);
+ if(ds < -eps){
+ S[DIM-1] = -1;
+ } else if(ds < eps) { // close to 0 let U * VT decide
+ const TooN::DefaultPrecision duv =
TooN::determinant_gaussian_elimination(svd.get_U())
+
* TooN::determinant_gaussian_elimination(svd.get_VT());
+ if(duv < 0)
+ S[DIM-1] = -1;
+ }
+
+ // compute trace(DS)
+ TooN::DefaultPrecision scale = 0;
+ for(int i = 0; i < DIM; ++i)
+ scale += svd.get_diagonal()[i] * S[i];
+
+ return std::make_pair(svd.get_U() * S.as_diagonal() * svd.get_VT(),
scale);
+}
+
+}
+
+/// computes the rotation between two sets of D-dim points maximizing b * Ta
+/// This function returns only the rotation matrix computed after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
+/// @param[in] a vector of D-dim points
+/// @param[in] b vector of D-dim points
+/// @return DxD matrix R describing the rotation such that b = R a
+/// @ingroup absorient
+template <int D>
+inline TooN::Matrix<D> computeRotation( const std::vector<TooN::Vector<D> > &
a, const std::vector<TooN::Vector<D> > & b ){
+ std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs =
Internal::computeOrientationScale( a, b );
+ return Rs.first;
+}
+
+/// computes the rotation between two sets of 3D points maximizing b * Ta
+/// This function returns only the rotation matrix as an SO3<> computed 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
/// @ingroup absorient
-TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > & a, const
std::vector<TooN::Vector<3> > & b );
+inline TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > &
a, const std::vector<TooN::Vector<3> > & b ){
+ std::pair<TooN::Matrix<3>, TooN::DefaultPrecision> result =
Internal::computeOrientationScale( a, b );
+ return TooN::SO3<>(result.first);
+}
+
+/// computes the rotation between two sets of 2D points maximizing b * Ta
+/// This function returns only the rotation matrix as an SO3<> computed after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
+/// @param[in] a vector of 2D points
+/// @param[in] b vector of 2D points
+/// @return TooN::SO3 containing the rotation such that b = T a
+/// @ingroup absorient
+inline TooN::SO2<> computeOrientation( const std::vector<TooN::Vector<2> > &
a, const std::vector<TooN::Vector<2> > & b ){
+ std::pair<TooN::Matrix<2>, TooN::DefaultPrecision> result =
Internal::computeOrientationScale( a, b );
+ return TooN::SO2<>(result.first);
+}
/// directly computes a rotation between two pairs of rays in space maximizing
b * T a
/// its about 8x faster then using the general computeOrientation for 2
correspondences.
@@ -34,25 +110,140 @@
/// @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
+/// computes the rigid transformation between two corresponding D dimensional
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]
+/// The result is a rotation matrix R and a translation vector t
+/// that map points from vector a to points from vector b : b[i] = R * a[i] + t
+/// @param[in] a vector of D-dim points
+/// @param[in] b vector of D-dim points
+/// @return std::pair containing R and t such that b[i] = R * a[i] + t
+/// @ingroup absorient
+template <int D>
+std::pair<TooN::Matrix<D>, TooN::Vector<D> > computeAbsoluteOrientation( const
std::vector<TooN::Vector<D> > & a, const std::vector<TooN::Vector<D> > & b){
+ TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
+ const int DIM = a.front().size();
+ const size_t N = std::min(a.size(), b.size());
+
+ if(N == 1){ // quick special case
+ TooN::Matrix<D> R(DIM, DIM);
+ R = TooN::Identity;
+ return std::make_pair(R, b.front() - a.front());
+ }
+
+ // compute centroids
+ TooN::Vector<D> ma = TooN::Zeros(DIM), mb = TooN::Zeros(DIM);
+ for(unsigned i = 0; i < N; ++i){
+ ma += a[i];
+ mb += b[i];
+ }
+ ma /= N;
+ mb /= N;
+
+ // compute shifted locations
+ std::vector<TooN::Vector<D> > ap(N), bp(N);
+ for( unsigned i = 0; i < N; ++i){
+ ap[i] = a[i] - ma;
+ bp[i] = b[i] - ma;
+ }
+
+ // put resulting transformation together
+ std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs =
Internal::computeOrientationScale( ap, bp );
+ return std::make_pair(Rs.first, mb - Rs.first * ma);
+}
+
+/// overload of @ref computeAbsoluteOrientation that computes the rigid
transformation between two corresponding 3D point sets
+/// as 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
-/// @return TooN::SE3 containing the transformation such that b = T a
+/// @return TooN::SE3 T containing the transformation such that b = T a
/// @ingroup absorient
-TooN::SE3<> computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > &
a, const std::vector<TooN::Vector<3> > & b);
+inline TooN::SE3<> computeAbsoluteOrientation( const
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
+ std::pair<TooN::Matrix<3>, TooN::Vector<3> > Rt =
computeAbsoluteOrientation<3>( a, b );
+ return TooN::SE3<>(Rt.first, Rt.second);
+}
+
+/// overload of @ref computeAbsoluteOrientation that computes the rigid
transformation between two corresponding 2D point sets
+/// as an SE2 that maps points from vector a to points from vector b : b[i] =
SE2 * a[i]
+/// @param[in] a vector of 2D points
+/// @param[in] b vector of 2D points
+/// @return TooN::SE2 T containing the transformation such that b = T a
+/// @ingroup absorient
+inline TooN::SE2<> computeAbsoluteOrientation( const
std::vector<TooN::Vector<2> > & a, const std::vector<TooN::Vector<2> > & b){
+ std::pair<TooN::Matrix<2>, TooN::Vector<2> > Rt =
computeAbsoluteOrientation<2>( a, b );
+ return TooN::SE2<>(Rt.first, Rt.second);
+}
-/// computes a similarity transformation between two corresponding point sets
after
+/// computes the rigid transformation between two corresponding D dimensional
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]
+/// The result is a rotation matrix R, a scale factor s and a translation
vector t
+/// that map points from vector a to points from vector b : b[i] = s * R *
a[i] + t
+/// @param[in] a vector of D-dim points
+/// @param[in] b vector of D-dim points
+/// @return std::tuple containing R, t and s such that b[i] = s * R * a[i] + t
+/// @ingroup absorient
+template <int D>
+std::tr1::tuple<TooN::Matrix<D>, TooN::Vector<D>, TooN::DefaultPrecision >
computeSimilarity( const std::vector<TooN::Vector<D> > & a, const
std::vector<TooN::Vector<D> > & b){
+ TooN::SizeMismatch<D,D>::test(a.front().size(), b.front().size());
+ const int DIM = a.front().size();
+ const size_t N = std::min(a.size(), b.size());
+
+ if(N == 1){ // quick special case
+ TooN::Matrix<D> R(DIM, DIM);
+ R = TooN::Identity;
+ return std::tr1::make_tuple(R, b.front() - a.front(), 1);
+ }
+
+ // compute centroids
+ TooN::Vector<D> ma = TooN::Zeros(DIM), mb = TooN::Zeros(DIM);
+ for(unsigned i = 0; i < N; ++i){
+ ma += a[i];
+ mb += b[i];
+ }
+ ma /= N;
+ mb /= N;
+
+ // compute shifted locations
+ std::vector<TooN::Vector<D> > ap(N), bp(N);
+ for( unsigned i = 0; i < N; ++i){
+ ap[i] = a[i] - ma;
+ bp[i] = b[i] - ma;
+ }
+
+ // put resulting transformation together
+ std::pair<TooN::Matrix<D>, TooN::DefaultPrecision> Rs =
Internal::computeOrientationScale( ap, bp );
+ // compute scale
+ TooN::DefaultPrecision sa = 0;
+ for( unsigned int i = 0; i < N; ++i){
+ sa += TooN::norm_sq(ap[i]);
+ }
+ sa /= N;
+ const TooN::DefaultPrecision scale = Rs.second / sa;
+ return std::tr1::make_tuple(Rs.first, mb - Rs.first * (scale * ma),
scale);
+}
+
+/// overload of @ref computeSimilarity that computes the rigid transformation
between two corresponding 3D point sets
+/// as 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<>, TooN::DefaultPrecision> computeSimilarity( const
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b);
+inline TooN::SIM3<> computeSimilarity( const std::vector<TooN::Vector<3> > &
a, const std::vector<TooN::Vector<3> > & b){
+ std::tr1::tuple<TooN::Matrix<3>, TooN::Vector<3>,
TooN::DefaultPrecision > Rts = computeSimilarity<3>(a,b);
+ return TooN::SIM3<>(TooN::SO3<>(std::tr1::get<0>(Rts)),
std::tr1::get<1>(Rts), std::tr1::get<2>(Rts));
+}
+
+/// overload of @ref computeSimilarity that computes the rigid transformation
between two corresponding 2D point sets
+/// as an SE2 and a scale S that maps points from vector a to points from
vector b : b[i] = SE2 * S * a[i]
+/// @param[in] a vector of 2D points
+/// @param[in] b vector of 2D points
+/// @return a pair consisting of a TooN::SE2 T and a double S containing the
transformation such that b = T * S * a
+/// @ingroup absorient
+inline TooN::SIM2<> computeSimilarity( const std::vector<TooN::Vector<2> > &
a, const std::vector<TooN::Vector<2> > & b){
+ std::tr1::tuple<TooN::Matrix<2>, TooN::Vector<2>,
TooN::DefaultPrecision > Rts = computeSimilarity<2>(a,b);
+ return TooN::SIM2<>(TooN::SO2<>(std::tr1::get<0>(Rts)),
std::tr1::get<1>(Rts), std::tr1::get<2>(Rts));
+}
/// 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
Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.11
retrieving revision 1.12
diff -u -b -r1.11 -r1.12
--- src/absorient.cpp 27 Jun 2011 14:56:24 -0000 1.11
+++ src/absorient.cpp 29 Jun 2011 20:34:45 -0000 1.12
@@ -2,14 +2,11 @@
#include <cassert>
-#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;
@@ -25,44 +22,6 @@
return result;
}
-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
- TooN::Matrix<3> s = TooN::Zeros;
- for( size_t i = 0; i < N; i++){
- s += b[i].as_col() * a[i].as_row();
- }
- s /= N;
-
- // 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
static inline TooN::SO3<> canonicalOrientation( const TooN::Vector<3> & a,
const TooN::Vector<3> & b ){
TooN::Vector<3> n = a ^ b;
@@ -86,82 +45,6 @@
return TooN::SO3<> ::exp(diff.ln() * 0.5) * rAB;
}
-TooN::SE3<> computeAbsoluteOrientation( 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 TooN::SE3<>(TooN::SO3<>(), b[0] - a[0]);
- }
-
- 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 );
- result.get_translation() = mb - result.get_rotation() * ma;
- return result;
-}
-
-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
- 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;
- std::pair<TooN::SO3<>, TooN::DefaultPrecision> rs =
computeOrientationScale( ap, bp );
- result.get_rotation() = rs.first;
-
- std::cout << rs.second << std::endl;
-
- // compute scale
- TooN::DefaultPrecision sa = 0;
- for( unsigned int i = 0; i < N; ++i){
- sa += norm_sq(ap[i]);
- }
- sa /= N;
- const TooN::DefaultPrecision scale = rs.second / 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: /cvsroot/toon/tag/test/sim.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- test/sim.cpp 25 Jun 2011 09:14:57 -0000 1.2
+++ test/sim.cpp 29 Jun 2011 20:34:45 -0000 1.3
@@ -8,7 +8,6 @@
using namespace tag;
typedef vector<Vector<3> > Points;
-typedef pair<SE3<>, double> Sim;
double inline rand_u(){
return (double)rand()/RAND_MAX;
@@ -32,28 +31,28 @@
Points a(N), b(N), c(N);
- SE3<> pose(makeVector(1,2,3,2,1,0.5));
- const double scale = 0.37;
+ SIM3<> pose(makeVector(1,2,3,2,1,0.5, 0.37));
for( unsigned i = 0; i < N; ++i ){
a[i] = rand_vect<3>(-10, 10);
- b[i] = pose * (scale * a[i]);
+ b[i] = pose * a[i];
c[i] = b[i] + rand_vect<3>(-0.01, 0.01);
}
- Sim sb = computeSimilarity(a,b);
- Sim sc = computeSimilarity(a,c);
+ SIM3<> sb = computeSimilarity(a,b);
+ SIM3<> 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]));
+ eb += norm_sq(b[i] - sb * a[i]);
+ ec += norm_sq(c[i] - sc * a[i]);
}
+ eb = sqrt(eb/N);
+ ec = sqrt(ec/N);
-
- cout << "test\t" << pose.ln() << "\t" << scale << "\n";
- cout << "exact\t" << sb.first.ln() << "\t" << sb.second << "\t" << sqrt(
eb ) << "\n"
- << "noisy\t" << sc.first.ln() << "\t" << sc.second << "\t" << sqrt(
ec ) << endl;
+ cout << "test\t" << pose.ln() << "\n";
+ cout << "exact\t" << sb.ln() << "\t" << eb << "\n"
+ << "noisy\t" << sc.ln() << "\t" << ec << endl;
return 0;
}
- [Toon-members] tag tag/absorient.h src/absorient.cpp test/sim.cpp,
Gerhard Reitmayr <=