[Toon-members] tag libtool src/threepointpose.cpp tag/threepoi...

[Qi Pan]

CVSROOT:        /sources/toon
Module name:    tag
Changes by:     Qi Pan <qpan>   10/06/17 11:38:19

Modified files:
        .              : libtool 
        src            : threepointpose.cpp 
        tag            : threepointpose.h 

Log message:
        added interface for three point pose to allow using 3 rays instead of 3 
perspective projected points

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/libtool?cvsroot=toon&r1=1.21&r2=1.22
http://cvs.savannah.gnu.org/viewcvs/tag/src/threepointpose.cpp?cvsroot=toon&r1=1.3&r2=1.4
http://cvs.savannah.gnu.org/viewcvs/tag/tag/threepointpose.h?cvsroot=toon&r1=1.3&r2=1.4

Patches:
Index: libtool
===================================================================
RCS file: /sources/toon/tag/libtool,v
retrieving revision 1.21
retrieving revision 1.22
diff -u -b -r1.21 -r1.22
--- libtool     13 Jan 2010 16:28:21 -0000      1.21
+++ libtool     17 Jun 2010 11:38:18 -0000      1.22
@@ -2,7 +2,7 @@
 
 # libtool - Provide generalized library-building support services.
 # Generated automatically by config.status (tag) 0.1
-# Libtool was configured on host optimus:
+# Libtool was configured on host prime:
 # NOTE: Changes made to this file will be lost: look at ltmain.sh.
 #
 #   Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2003, 2004, 2005,
@@ -234,10 +234,10 @@
 hardcode_into_libs=yes
 
 # Compile-time system search path for libraries.
-sys_lib_search_path_spec="/usr/lib64/gcc/x86_64-suse-linux/4.3 /usr/lib64 
/lib64 /usr/x86_64-suse-linux/lib"
+sys_lib_search_path_spec="/usr/lib64/gcc/x86_64-suse-linux/4.4 /usr/lib64 
/lib64 /usr/x86_64-suse-linux/lib"
 
 # Run-time system search path for libraries.
-sys_lib_dlsearch_path_spec="/lib /usr/lib /usr/X11R6/lib64/Xaw3d 
/usr/X11R6/lib64 /usr/lib64/Xaw3d /usr/X11R6/lib/Xaw3d /usr/X11R6/lib 
/usr/lib/Xaw3d /usr/x86_64-suse-linux/lib /usr/local/lib /opt/kde3/lib /lib64 
/lib /usr/lib64 /usr/lib /usr/local/lib64 /opt/kde3/lib64 /usr/lib64/graphviz 
/usr/lib64/graphviz/sharp /usr/lib64/graphviz/java /usr/lib64/graphviz/perl 
/usr/lib64/graphviz/php /usr/lib64/graphviz/ocaml /usr/lib64/graphviz/python 
/usr/lib64/graphviz/lua /usr/lib64/graphviz/tcl /usr/lib64/graphviz/guile 
/usr/lib64/graphviz/ruby /usr/i486-linux-libc5/lib "
+sys_lib_dlsearch_path_spec="/lib /usr/lib /usr/X11R6/lib64/Xaw3d 
/usr/X11R6/lib64 /usr/lib64/Xaw3d /usr/X11R6/lib/Xaw3d /usr/X11R6/lib 
/usr/lib/Xaw3d /usr/x86_64-suse-linux/lib /usr/local/lib /opt/kde3/lib /lib64 
/lib /usr/lib64 /usr/lib /usr/local/lib64 /opt/kde3/lib64 /usr/lib64/graphviz 
/usr/lib64/graphviz/sharp /usr/lib64/graphviz/java /usr/lib64/graphviz/perl 
/usr/lib64/graphviz/php /usr/lib64/graphviz/ocaml /usr/lib64/graphviz/python 
/usr/lib64/graphviz/lua /usr/lib64/graphviz/tcl /usr/lib64/graphviz/guile 
/usr/lib64/graphviz/ruby /usr/local/cuda/lib64 /usr/local/cuda/lib /sbin 
/usr/sbin "
 
 # Whether dlopen is supported.
 dlopen_support=unknown
@@ -9040,17 +9040,17 @@
 hardcode_action=immediate
 
 # The directories searched by this compiler when creating a shared library.
-compiler_lib_search_dirs="/usr/lib64/gcc/x86_64-suse-linux/4.3 
/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../lib64 /lib/../lib64 
/usr/lib/../lib64 
/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../x86_64-suse-linux/lib 
/usr/lib64/gcc/x86_64-suse-linux/4.3/../../.."
+compiler_lib_search_dirs="/usr/lib64/gcc/x86_64-suse-linux/4.4 
/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../lib64 /lib/../lib64 
/usr/lib/../lib64 
/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../x86_64-suse-linux/lib 
/usr/lib64/gcc/x86_64-suse-linux/4.4/../../.."
 
 # Dependencies to place before and after the objects being linked to
 # create a shared library.
-predep_objects="/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../lib64/crti.o 
/usr/lib64/gcc/x86_64-suse-linux/4.3/crtbeginS.o"
-postdep_objects="/usr/lib64/gcc/x86_64-suse-linux/4.3/crtendS.o 
/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../lib64/crtn.o"
+predep_objects="/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../lib64/crti.o 
/usr/lib64/gcc/x86_64-suse-linux/4.4/crtbeginS.o"
+postdep_objects="/usr/lib64/gcc/x86_64-suse-linux/4.4/crtendS.o 
/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../lib64/crtn.o"
 predeps=""
 postdeps="-lstdc++ -lm -lgcc_s -lc -lgcc_s"
 
 # The library search path used internally by the compiler when linking
 # a shared library.
-compiler_lib_search_path="-L/usr/lib64/gcc/x86_64-suse-linux/4.3 
-L/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../lib64 -L/lib/../lib64 
-L/usr/lib/../lib64 
-L/usr/lib64/gcc/x86_64-suse-linux/4.3/../../../../x86_64-suse-linux/lib 
-L/usr/lib64/gcc/x86_64-suse-linux/4.3/../../.."
+compiler_lib_search_path="-L/usr/lib64/gcc/x86_64-suse-linux/4.4 
-L/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../lib64 -L/lib/../lib64 
-L/usr/lib/../lib64 
-L/usr/lib64/gcc/x86_64-suse-linux/4.4/../../../../x86_64-suse-linux/lib 
-L/usr/lib64/gcc/x86_64-suse-linux/4.4/../../.."
 
 # ### END LIBTOOL TAG CONFIG: CXX

Index: src/threepointpose.cpp
===================================================================
RCS file: /sources/toon/tag/src/threepointpose.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- src/threepointpose.cpp      29 Apr 2009 18:48:57 -0000      1.3
+++ src/threepointpose.cpp      17 Jun 2010 11:38:19 -0000      1.4
@@ -115,4 +115,89 @@
     return count;
 }
 
+int three_point_pose(const TooN::Vector<3> xi[], const TooN::Vector<3> rays[], 
std::vector<TooN::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(rays[0]), zb = unit(rays[1]), zc = unit(rays[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;
+}
+
 }

Index: tag/threepointpose.h
===================================================================
RCS file: /sources/toon/tag/tag/threepointpose.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- tag/threepointpose.h        29 Apr 2009 18:48:58 -0000      1.3
+++ tag/threepointpose.h        17 Jun 2010 11:38:19 -0000      1.4
@@ -22,6 +22,18 @@
 /// @ingroup posegroup
 int three_point_pose(const TooN::Vector<3> x[], const TooN::Vector<2> z[], 
std::vector<TooN::SE3<> >& poses);
 
+/// 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 3D vectors 
that are rays corresponding to (unprojected) 2D observations of the 3D 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 rays corresponding to image points 
cast 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
+/// @ingroup posegroup
+int three_point_pose(const TooN::Vector<3> x[], const TooN::Vector<3> rays[], 
std::vector<TooN::SE3<> >& poses);
+
 }
 
 #endif