[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/src fourpointpose.cpp
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag/src fourpointpose.cpp |
Date: |
Sun, 04 Jun 2006 12:21:09 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 06/06/04 12:21:09
Modified files:
src : fourpointpose.cpp
Log message:
refactored common code to helper function
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/fourpointpose.cpp?cvsroot=toon&r1=1.2&r2=1.3
Patches:
Index: fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- fourpointpose.cpp 4 Jun 2006 12:09:19 -0000 1.2
+++ fourpointpose.cpp 4 Jun 2006 12:21:09 -0000 1.3
@@ -22,7 +22,7 @@
}
// helper function for @ref fourPointPose computing the coefficients of the A
matrix
-static TooN::Vector<5> getACoeffs(double c1, double c2, double c3, double d1,
double d2, double d3){
+TooN::Vector<5> getACoeffs(double c1, double c2, double c3, double d1, double
d2, double d3){
TooN::Vector<5> coeffs;
// x^4 coefficient
coeffs[4] = (c3*c3 - c1*c2*c3 - 4 + c1*c1 + c2*c2);
@@ -63,7 +63,7 @@
}
// helper function for @ref fourPointPose computing the coefficients of the B
matrix
-static TooN::Vector<3> getBCoeffs( int i, int j, int k, int l, const
TooN::Vector<5> & v4, const TooN::Vector<5> & v5 ){
+TooN::Vector<3> getBCoeffs( int i, int j, int k, int l, const TooN::Vector<5>
& v4, const TooN::Vector<5> & v5 ){
TooN::Vector<3> coeff;
coeff[0] = v4[i]*v4[j] - v4[k]*v4[l];
coeff[1] = v4[i]*v5[j] + v5[i]*v4[j] - (v4[k]*v5[l] + v5[k]*v4[l]);
@@ -71,8 +71,8 @@
return coeff;
}
-// this code is copied in the next function, modifications need to be made
there too
-TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+// contains the main computational part common to both of the high level
functions
+static bool fourPointSolver ( const std::vector<TooN::Vector<3> > & points,
std::vector<TooN::Vector<3> > & myPixels, TooN::Vector<6> & distances,
std::vector<TooN::Vector<2> > & length){
TooN::Matrix<5> A;
TooN::Vector<5> v4, v5;
TooN::Matrix<7,3> B;
@@ -80,15 +80,10 @@
TooN::Vector<5> t;
TooN::Matrix<3> vecsCamera, vecsWorld;
- TooN::Vector<6> distances;
TooN::Vector<6> angles;
- double orientationTest = ((points[1] - points[0]) ^ (points[2] -
points[0])) * (points[3] - points[0]);
-
// normalising scales for angle computation in next loop
- std::vector<TooN::Vector<3> > myPixels(4);
for(unsigned int i = 0; i < 4; i++){
- myPixels[i] = pixels[i];
TooN::normalize(myPixels[i]);
}
@@ -138,13 +133,27 @@
if( xx < 0)
xx *= -1;
- valid = true;
- std::vector<TooN::Vector<2> > length(4);
+ bool valid = true;
double x = sqrt( xx );
length[0] = (TooN::make_Vector, x, -x); // possible distances to point 0
valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1] );
valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2] );
valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3] );
+ return valid;
+}
+
+TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+ double orientationTest = ((points[1] - points[0]) ^ (points[2] -
points[0])) * (points[3] - points[0]);
+
+ // normalising scales for angle computation in next loop
+ std::vector<TooN::Vector<3> > myPixels(4);
+ for(unsigned int i = 0; i < 4; i++)
+ myPixels[i] = pixels[i];
+
+ // resulting possible distances
+ std::vector<TooN::Vector<2> > length(4);
+ TooN::Vector<6> distances;
+ valid = fourPointSolver( points, myPixels, distances, length);
if( !valid )
return TooN::SE3();
@@ -191,77 +200,16 @@
}
// just a copy of the above code with modifications to the case search
-// could be refactored, but that breaks the nice algorithmic structure
TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > &
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid ){
- TooN::Matrix<5> A;
- TooN::Vector<5> v4, v5;
- TooN::Matrix<7,3> B;
- TooN::Vector<3> bnull;
- TooN::Vector<5> t;
- TooN::Matrix<3> vecsCamera, vecsWorld;
-
- TooN::Vector<6> distances;
- TooN::Vector<6> angles;
-
// normalising scales for angle computation in next loop
std::vector<TooN::Vector<3> > myPixels(4);
- for(unsigned int i = 0; i < 4; i++){
+ for(unsigned int i = 0; i < 4; i++)
myPixels[i] = pixels[i];
- TooN::normalize(myPixels[i]);
- }
-
- int count = 0;
- for( unsigned int i = 0; i < 3; i++)
- for( unsigned int j = i+1; j < 4; j++, count++){
- TooN::Vector<3> diff = points[i] - points[j];
- distances[count] = diff * diff;
- angles[count] = 2* myPixels[i] * myPixels[j];
- }
-
- TooN::Zero(A);
- A.slice<0,0,1,5>() = getACoeffs(angles[0], angles[1], angles[3],
distances[0], distances[1], distances[3]).as_row();
- A.slice<1,0,1,5>() = getACoeffs(angles[0], angles[2], angles[4],
distances[0], distances[2], distances[4]).as_row();
- A.slice<2,0,1,5>() = getACoeffs(angles[1], angles[2], angles[5],
distances[1], distances[2], distances[5]).as_row();
- TooN::SVD<5> svdA(A);
- v4.as_row() = svdA.get_VT().slice<3,0,1,5>();
- v5.as_row() = svdA.get_VT().slice<4,0,1,5>();
-
- B.slice<0,0,1,3>() = getBCoeffs(4,2,3,3, v4, v5).as_row();
- B.slice<1,0,1,3>() = getBCoeffs(4,1,3,2, v4, v5).as_row();
- B.slice<2,0,1,3>() = getBCoeffs(4,0,3,1, v4, v5).as_row();
- B.slice<3,0,1,3>() = getBCoeffs(4,0,2,2, v4, v5).as_row();
- B.slice<4,0,1,3>() = getBCoeffs(3,1,2,2, v4, v5).as_row();
- B.slice<5,0,1,3>() = getBCoeffs(3,0,2,1, v4, v5).as_row();
- B.slice<6,0,1,3>() = getBCoeffs(2,0,1,1, v4, v5).as_row();
-
- TooN::SVD<7,3> svdB(B);
- bnull.as_row() = svdB.get_VT().slice<2,0,1,3>();
-
- double lambda, roh;
- if( bnull[1] != 0 ){ // lambda * roh != 0 => both are != 0
- double ratio = (bnull[0]/ bnull[1] + bnull[1] / bnull[2]) * 0.5;
- roh = 1.0 / (v4[0] * ratio + v5[0]);
- lambda = ratio * roh;
- } else if( bnull[2] != 0 ) { // roh != 0 => lambda == 0
- lambda = 0;
- roh = 1/v5[0];
- } else { // lambda != 0 and roh == 0
- roh = 0;
- lambda = 1/v4[0];
- }
-
- t = v4 * lambda + v5 * roh;
- double xx = (t[1]/t[0] + t[2]/t[1] + t[3]/t[2] + t[4]/t[3]) / 4;
- if( xx < 0)
- xx *= -1;
-
- valid = true;
+ // resulting possible distances
std::vector<TooN::Vector<2> > length(4);
- double x = sqrt( xx );
- valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1] );
- valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2] );
- valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3] );
+ TooN::Vector<6> distances;
+ valid = fourPointSolver( points, myPixels, distances, length);
if( !valid )
return TooN::SE3();
@@ -271,8 +219,8 @@
double minError = 1e10;
int minIndex = -1;
// we assume now that the point is in front of the camera and therefore
the solution is the one with
- // positive x
- testPoint[0] = myPixels[0] * x;
+ // positive x (which is the first in the vector)
+ testPoint[0] = myPixels[0] * length[0][0];
for(unsigned int h = 0; h < 8; h++){
testPoint[1] = myPixels[1] * length[1][!!(h&1)];
testPoint[2] = myPixels[2] * length[2][!!(h&2)];
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/src fourpointpose.cpp,
Gerhard Reitmayr <=