[Toon-members] tag Makefile.am tag/quartic.h tag/threepointpos...

[Ethan Eade]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Ethan Eade <ethaneade>  08/03/31 16:35:42

Modified files:
        .              : Makefile.am 
Added files:
        tag            : quartic.h threepointpose.h 
        src            : quartic.cpp threepointpose.cpp 

Log message:
        An efficient implementation of the perspective three point pose 
algorithm
        given by Fischer and Bolles, 1980.  The quartic solver uses a careful
        implementation of the quartic formula, and the resulting roots are 
refined
        using Newton's method.  On my hardware, the implementation takes about 3
        microseconds to run in the average case.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/quartic.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/tag/threepointpose.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/src/quartic.cpp?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/src/threepointpose.cpp?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/Makefile.am?cvsroot=toon&r1=1.8&r2=1.9

Patches:
Index: Makefile.am
===================================================================
RCS file: /cvsroot/toon/tag/Makefile.am,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- Makefile.am 3 Dec 2007 17:39:30 -0000       1.8
+++ Makefile.am 31 Mar 2008 16:35:42 -0000      1.9
@@ -1,9 +1,12 @@
 # the library we want to create and its source files
+
 lib_LTLIBRARIES     = libtoontag.la
-libtoontag_la_SOURCES   = src/absorient.cpp src/fourpointpose.cpp
+libtoontag_la_SOURCES   = src/quartic.cpp src/threepointpose.cpp 
src/absorient.cpp src/fourpointpose.cpp  
 
 # the header files to be installed
 pkginclude_HEADERS  = \
+       tag/quartic.h \
+       tag/threepointpose.h \
        tag/absorient.h \
        tag/array.h \
        tag/fourpointpose.h \

Index: tag/quartic.h
===================================================================
RCS file: tag/quartic.h
diff -N tag/quartic.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ tag/quartic.h       31 Mar 2008 16:35:41 -0000      1.1
@@ -0,0 +1,31 @@
+#ifndef QUARTIC_H
+#define QUARTIC_H
+
+/// A function to evaluate x^4 + Bx^3 + Cx^2 + Dx + E
+inline double eval_quartic(double B, double C, double D, double E, double x)
+{
+    return E + x*(D + x*(C + x*(B + x)));
+}
+
+/// A function that performs one iteration of Newton's method on the quartic 
x^4 + Bx^3 + Cx^2 + Dx + E
+inline double newton_quartic(double B, double C, double D, double E, double x)
+{
+    double fx = E + x*(D + x*(C + x*(B + x)));
+    double dx = D + x*(2*C + x*(3*B + x*4));
+    return x - fx/dx;
+}
+
+
+/// A function to find the real roots of a quartic polynomial x^4 + Bx^3 + 
Cx^2 + Dx + E.
+/// It efficiently implements the quartic formula as given by Cardano, 
Harriot, et al.
+/// The precision of the resulting roots depends on the nature of the 
coefficients. 
+/// Sufficient precision can be ensured by refining the resulting roots using 
Newton's method.
+/// @param[in] B the coefficient of the cubic term
+/// @param[in] C the coefficient of the quadratic term
+/// @param[in] D the coefficient of the linear term
+/// @param[in] E the coefficient of the constant term
+/// @param[out] r an array in which 0, 2, or 4 real roots will be stored
+/// @return the number of real roots
+int find_quartic_real_roots(double B, double C, double D, double E, double 
r[]);
+
+#endif

Index: tag/threepointpose.h
===================================================================
RCS file: tag/threepointpose.h
diff -N tag/threepointpose.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ tag/threepointpose.h        31 Mar 2008 16:35:41 -0000      1.1
@@ -0,0 +1,19 @@
+#ifndef THREEPOINTPOSE_H
+#define THREEPOINTPOSE_H
+
+#include <TooN/se3.h>
+#include <vector>
+
+/// The function for pose estimation from three 2D - 3D point correspondences.
+/// It implements the algorithm given by the Fischer and Bolles RANSAC paper, 
1980.
+/// This function assumes that the three points are in general position (not 
collinear).
+/// Input is an array of 3D cartesian positions and an array of 2D vectors 
that are the perspective projections of the points.
+/// Ouput is up to four poses satisfying the input with positive depths 
(points in front of the camera).
+/// @param[in] x an array containing at least 3 points
+/// @param[in] z an array containing the perspective projections of the points 
given by x in the current pose
+/// @param[out] poses the vector onto which any valid poses are appended
+/// @return the number of  poses appended to the vector
+
+int three_point_pose(const TooN::Vector<3> x[], const TooN::Vector<2> z[], 
std::vector<TooN::SE3>& poses);
+
+#endif

Index: src/quartic.cpp
===================================================================
RCS file: src/quartic.cpp
diff -N src/quartic.cpp
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ src/quartic.cpp     31 Mar 2008 16:35:41 -0000      1.1
@@ -0,0 +1,78 @@
+#include <util/quartic.h>
+
+#include <cmath>
+#include <cassert>
+#include <algorithm>
+using namespace std;
+
+int depressed_cubic_real_roots(double P, double Q, double r[])
+{
+    static const double third = 1.0/3.0;
+    double third_P = third * P;
+    double disc = 4 * (third_P*third_P*third_P) + Q*Q;
+    if (disc >= 0) {
+       double root_disc = sqrt(disc);
+       double cube = Q < 0 ? -0.5 * (Q - root_disc) : -0.5 * (Q + root_disc);
+       double u = cbrt(cube);
+       double x = u - third_P / u;
+       r[0] = x;
+       return 1;
+    } else {
+       double y3_re = -0.5  * Q;
+       double y3_im = 0.5 * sqrt(-disc);
+       // y = cube root (y3)
+       double theta = atan2(y3_im, y3_re) * third;
+       double radius = sqrt(-third_P);
+       double y_re = radius * cos(theta);
+       double x = 2*y_re;
+       double root_disc = sqrt(-3*x*x - 4*P);
+       if (x > 0) {
+           r[0] = -0.5 * (x + root_disc);
+           r[1] = -0.5 * (x - root_disc);
+           r[2] = x;
+       } else {
+           r[0] = x;
+           r[1] = -0.5 * (x + root_disc);
+           r[2] = -0.5 * (x - root_disc);
+       }
+       return 3;
+    }    
+}
+
+int find_quartic_real_roots(double B, double C, double D, double E, double r[])
+{
+    static const double third = 1.0/3.0;
+    double alpha = C - 0.375 * B * B;
+    double half_B = 0.5 * B;
+    double beta = half_B *(half_B*half_B - C) + D;
+    double q_B = 0.25 * B;
+    double gamma = q_B * (q_B *(C - 3 * q_B * q_B) - D) + E;
+    double third_alpha = alpha * third;
+    double P = -alpha * third_alpha * 0.25 - gamma;
+    double Q = third_alpha * (-third_alpha * third_alpha * 0.25 + gamma) - 
beta*beta*0.125;
+
+    double dcr[3];
+    int ndcr = depressed_cubic_real_roots(P,Q, dcr);
+    double y = dcr[ndcr-1] - third * 2.5 * alpha;
+
+    double disc2 = alpha + 2*y;
+    
+    double W = sqrt(disc2);
+
+    double disc_base = 2*alpha + disc2; //3*alpha + 2*y;
+    double disc_add = 2*beta / W;
+    double x_base = -0.25 * B;
+
+    int count = 0;
+    if (disc_base + disc_add <= 0) {
+       double sr = sqrt(-disc_base-disc_add);
+       r[count++] = x_base + 0.5 * (W - sr);
+       r[count++] = x_base + 0.5 * (W + sr);
+    }
+    if (disc_base - disc_add <= 0) {
+       double sr = sqrt(-disc_base + disc_add);
+       r[count++] = x_base - 0.5 * (W + sr);   
+       r[count++] = x_base - 0.5 * (W - sr);
+    }
+    return count;
+}

Index: src/threepointpose.cpp
===================================================================
RCS file: src/threepointpose.cpp
diff -N src/threepointpose.cpp
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ src/threepointpose.cpp      31 Mar 2008 16:35:42 -0000      1.1
@@ -0,0 +1,116 @@
+#include <TooN/gaussian_elimination.h>
+#include <TooN/se3.h>
+#include <vector>
+#include <tag/quartic.h>
+#include <tag/threepointpose.h>
+using namespace TooN;
+using namespace std;
+
+
+inline double square(double x) { return x*x; }
+
+SE3 three_point_absolute_orientation(const Vector<3> x[], const Vector<3> y[])
+{
+    Matrix<3> D, D1;
+    
+    D[0] = x[1] - x[0];
+    D[1] = x[2] - x[0];
+    D[2] = D[1] ^ D[0];
+    
+    D1[0] = y[1] - y[0];
+    D1[1] = y[2] - y[0];
+    D1[2] = D1[1] ^ D1[0];
+    
+    
+    SO3 so3(gaussian_elimination(D, D1).T());
+    
+    Vector<3> T = y[0] - so3 * x[0];
+
+    return SE3(so3, T);
+}
+
+int three_point_pose(const Vector<3> xi[], const Vector<2> zi[], vector<SE3>& 
poses)
+{    
+    double ab_sq = norm_sq(xi[1] - xi[0]);
+    double ac_sq = norm_sq(xi[2] - xi[0]);
+    double bc_sq = norm_sq(xi[2] - xi[1]);
+
+    Vector<3> za = unit(unproject(zi[0])), zb = unit(unproject(zi[1])), zc = 
unit(unproject(zi[2]));
+    
+    double cos_ab = za*zb;
+    double cos_ac = za*zc;
+    double cos_bc = zb*zc;
+
+    double K1 = bc_sq / ac_sq;
+    double K2 = bc_sq / ab_sq;
+
+    double p[5];
+    {
+       
+       double K12 = K1*K2;
+       double K12_1_2 = K12 - K1 - K2;
+       double K2_1_K1_cab = K2*(1-K1)*cos_ab;
+       
+       
+       p[4] = (square(K12_1_2) 
+               - 4*K12*cos_bc*cos_bc);
+       p[3] = (4*(K12_1_2)*K2_1_K1_cab 
+               + 4*K1*cos_bc*((K12+K2-K1)*cos_ac 
+                              + 2*K2*cos_ab*cos_bc));
+       p[2] = (square(2*K2_1_K1_cab) + 
+               2*(K12 + K1 - K2)*K12_1_2 
+               + 4*K1*((K1-K2)*cos_bc*cos_bc 
+                       + (1-K2)*K1*cos_ac*cos_ac 
+                       - 2*K2*(1 + K1) *cos_ab*cos_ac*cos_bc));
+       p[1] = (4*(K12 + K1 - K2)*K2_1_K1_cab 
+               + 4*K1*((K12 - K1 + K2)*cos_ac*cos_bc
+                       + 2*K12*cos_ab*cos_ac*cos_ac));
+       p[0] = square(K12 + K1 - K2) - 4*K12*K1*cos_ac*cos_ac;
+    }
+    Vector<3> xi1[3];
+
+    double roots[4];
+    double inv_p4 = 1.0 / p[4];
+    for (int i=0; i<4; ++i)
+       p[i] *= inv_p4;
+    int nr = find_quartic_real_roots(p[3], p[2], p[1], p[0], roots);
+
+    int count = 0;
+    for (int i=0; i<nr; ++i) {
+       double x = roots[i];
+       if (x <= 0)
+           continue;
+       for (int j=0; j<3; ++j)
+           x = newton_quartic(p[3], p[2], p[1], p[0], x);
+       double xx = x*x;
+
+       double a_den = xx - 2*x*cos_ab + 1;
+       double a = sqrt(ab_sq / a_den);
+       double b = a*x;
+
+       double M = 1 - K1;
+       double P = 2*(K1*cos_ac - x*cos_bc);
+       double Q = xx - K1;
+
+       double P1 = -2*x*cos_bc;
+       double Q1 = xx - K2*a_den;
+       
+       double den = M*Q1 - Q;
+       if (den == 0) {
+           cerr << "skipped" << endl;
+           continue;
+       }
+       
+       double y = (P1*Q - P*Q1) / den;
+       double c = a * y;
+
+       xi1[0] = a*za;
+       xi1[1] = b*zb;
+       xi1[2] = c*zc;
+
+       poses.push_back(three_point_absolute_orientation(xi, xi1));     
+       ++count;
+    }
+    return count;
+}
+