[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag Makefile.am Makefile.in src/five_point.cpp ...
|
From: |
Edward Rosten |
|
Subject: |
[Toon-members] tag Makefile.am Makefile.in src/five_point.cpp ... |
|
Date: |
Fri, 15 May 2009 14:54:31 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Edward Rosten <edrosten> 09/05/15 14:54:31
Modified files:
. : Makefile.am Makefile.in
src : five_point.cpp
tag : five_point.h
Added files:
src : polynomial.cc polynomial.h
Removed files:
src : solve.h sturm.c util.c
Log message:
4x speedup to five_point. The new code will give slightly different
results
to the old code, due to differences in numerics, tolerances and etc.
The nullspace is now found using Gauss-Jordan, instead of SVD (for a
speedup
of about 30x for this particular part).
New hand written polynomial root finder, for a speedup of 4x and
improved
reliability (the old one would occasionally expect -1 roots due to
numerical
instabilities and fail with an alloc_error).
The new root-finder will not attempt to find roots of p(x) for |x|>100,
since
it is not possible to find these root to high enough accuracy.
Roots are bracketed using Sturm sequences (as before) but using the
resursive
implementation which is more efficient.
The root polishing uses a hybrid Newton-Raphson/bisection rootfinder
which
usually gets roots to higher accuracy than the old bisection solver and
almost
always converges in < 8 iterations, and often in 4. It will give up
after 10
iterations.
A new tolerance feps is introduced, so that for a numerical root at x,
if
|p(x)| > feps, the root will not be recorded.
The other tolerances (eps and zeps) follow the convention of NR in C.
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/Makefile.am?cvsroot=toon&r1=1.14&r2=1.15
http://cvs.savannah.gnu.org/viewcvs/tag/Makefile.in?cvsroot=toon&r1=1.14&r2=1.15
http://cvs.savannah.gnu.org/viewcvs/tag/src/five_point.cpp?cvsroot=toon&r1=1.18&r2=1.19
http://cvs.savannah.gnu.org/viewcvs/tag/src/polynomial.cc?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/src/polynomial.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/tag/src/solve.h?cvsroot=toon&r1=1.1&r2=0
http://cvs.savannah.gnu.org/viewcvs/tag/src/sturm.c?cvsroot=toon&r1=1.2&r2=0
http://cvs.savannah.gnu.org/viewcvs/tag/src/util.c?cvsroot=toon&r1=1.2&r2=0
http://cvs.savannah.gnu.org/viewcvs/tag/tag/five_point.h?cvsroot=toon&r1=1.8&r2=1.9
Patches:
Index: Makefile.am
===================================================================
RCS file: /cvsroot/toon/tag/Makefile.am,v
retrieving revision 1.14
retrieving revision 1.15
diff -u -b -r1.14 -r1.15
--- Makefile.am 23 Apr 2009 17:42:42 -0000 1.14
+++ Makefile.am 15 May 2009 14:54:29 -0000 1.15
@@ -8,7 +8,7 @@
lib_LTLIBRARIES = libtoontag.la
libtoontag_la_SOURCES = src/quartic.cpp src/threepointpose.cpp
src/absorient.cpp \
- src/handeye.cpp src/five_point.cpp
src/five_point_buildmatrix.cpp src/sturm.c src/util.c
+ src/handeye.cpp src/five_point.cpp
src/five_point_buildmatrix.cpp src/polynomial.cc
# the header files to be installed
pkginclude_HEADERS = \
Index: Makefile.in
===================================================================
RCS file: /cvsroot/toon/tag/Makefile.in,v
retrieving revision 1.14
retrieving revision 1.15
diff -u -b -r1.14 -r1.15
--- Makefile.in 23 Apr 2009 17:42:42 -0000 1.14
+++ Makefile.in 15 May 2009 14:54:29 -0000 1.15
@@ -59,21 +59,12 @@
LTLIBRARIES = $(lib_LTLIBRARIES)
libtoontag_la_LIBADD =
am_libtoontag_la_OBJECTS = quartic.lo threepointpose.lo absorient.lo \
- handeye.lo five_point.lo five_point_buildmatrix.lo sturm.lo \
- util.lo
+ handeye.lo five_point.lo five_point_buildmatrix.lo \
+ polynomial.lo
libtoontag_la_OBJECTS = $(am_libtoontag_la_OBJECTS)
DEFAULT_INCLUDES = address@hidden@
depcomp = $(SHELL) $(top_srcdir)/depcomp
am__depfiles_maybe = depfiles
-COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
- $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS)
-LTCOMPILE = $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) \
- --mode=compile $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \
- $(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS)
-CCLD = $(CC)
-LINK = $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) \
- --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(AM_LDFLAGS) \
- $(LDFLAGS) -o $@
CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \
$(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS)
LTCXXCOMPILE = $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) \
@@ -212,7 +203,7 @@
AM_CXXFLAGS = $(WARN_OPTIONS) $(OPTIMIZATION_OPTIONS)
lib_LTLIBRARIES = libtoontag.la
libtoontag_la_SOURCES = src/quartic.cpp src/threepointpose.cpp
src/absorient.cpp \
- src/handeye.cpp src/five_point.cpp
src/five_point_buildmatrix.cpp src/sturm.c src/util.c
+ src/handeye.cpp src/five_point.cpp
src/five_point_buildmatrix.cpp src/polynomial.cc
# the header files to be installed
@@ -240,7 +231,7 @@
all: all-am
.SUFFIXES:
-.SUFFIXES: .c .cpp .lo .o .obj
+.SUFFIXES: .cc .cpp .lo .o .obj
am--refresh:
@:
$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps)
@@ -314,61 +305,25 @@
@AMDEP_TRUE@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
@AMDEP_TRUE@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
@AMDEP_TRUE@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
address@hidden@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
@AMDEP_TRUE@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
address@hidden@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
@AMDEP_TRUE@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
address@hidden@@am__include@ @address@hidden/$(DEPDIR)/address@hidden@
-
-.c.o:
address@hidden@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
address@hidden@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(COMPILE) -c $<
-
-.c.obj:
address@hidden@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@
`$(CYGPATH_W) '$<'`
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
address@hidden@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(COMPILE) -c `$(CYGPATH_W) '$<'`
-
-.c.lo:
address@hidden@ $(LTCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo
address@hidden@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=yes
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(LTCOMPILE) -c -o $@ $<
-
-sturm.lo: src/sturm.c
address@hidden@ $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -MT sturm.lo -MD -MP -MF $(DEPDIR)/sturm.Tpo
-c -o sturm.lo `test -f 'src/sturm.c' || echo '$(srcdir)/'`src/sturm.c
address@hidden@ mv -f $(DEPDIR)/sturm.Tpo $(DEPDIR)/sturm.Plo
address@hidden@@am__fastdepCC_FALSE@ source='src/sturm.c' object='sturm.lo'
libtool=yes @AMDEPBACKSLASH@
address@hidden@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -c -o sturm.lo `test -f 'src/sturm.c' ||
echo '$(srcdir)/'`src/sturm.c
-
-util.lo: src/util.c
address@hidden@ $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -MT util.lo -MD -MP -MF $(DEPDIR)/util.Tpo
-c -o util.lo `test -f 'src/util.c' || echo '$(srcdir)/'`src/util.c
address@hidden@ mv -f $(DEPDIR)/util.Tpo $(DEPDIR)/util.Plo
address@hidden@@am__fastdepCC_FALSE@ source='src/util.c' object='util.lo'
libtool=yes @AMDEPBACKSLASH@
address@hidden@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(LIBTOOL) --tag=CC $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) -c -o util.lo `test -f 'src/util.c' || echo
'$(srcdir)/'`src/util.c
-.cpp.o:
+.cc.o:
@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o
$@ $<
@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ $<
-.cpp.obj:
+.cc.obj:
@am__fastdepCXX_TRUE@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o
$@ `$(CYGPATH_W) '$<'`
@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
@am__fastdepCXX_FALSE@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'`
-.cpp.lo:
+.cc.lo:
@am__fastdepCXX_TRUE@ $(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c
-o $@ $<
@am__fastdepCXX_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=yes
@AMDEPBACKSLASH@
@@ -417,6 +372,34 @@
@AMDEP_TRUE@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
@am__fastdepCXX_FALSE@ $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o five_point_buildmatrix.lo `test -f
'src/five_point_buildmatrix.cpp' || echo
'$(srcdir)/'`src/five_point_buildmatrix.cpp
+polynomial.lo: src/polynomial.cc
address@hidden@ $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -MT polynomial.lo -MD -MP -MF
$(DEPDIR)/polynomial.Tpo -c -o polynomial.lo `test -f 'src/polynomial.cc' ||
echo '$(srcdir)/'`src/polynomial.cc
address@hidden@ mv -f $(DEPDIR)/polynomial.Tpo $(DEPDIR)/polynomial.Plo
address@hidden@@am__fastdepCXX_FALSE@ source='src/polynomial.cc'
object='polynomial.lo' libtool=yes @AMDEPBACKSLASH@
address@hidden@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(LIBTOOL) --tag=CXX $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS)
--mode=compile $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS)
$(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS) -c -o polynomial.lo `test -f
'src/polynomial.cc' || echo '$(srcdir)/'`src/polynomial.cc
+
+.cpp.o:
address@hidden@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
address@hidden@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(CXXCOMPILE) -c -o $@ $<
+
+.cpp.obj:
address@hidden@ $(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@
`$(CYGPATH_W) '$<'`
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
address@hidden@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=no
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'`
+
+.cpp.lo:
address@hidden@ $(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
address@hidden@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo
address@hidden@@am__fastdepCXX_FALSE@ source='$<' object='$@' libtool=yes
@AMDEPBACKSLASH@
address@hidden@@am__fastdepCXX_FALSE@ DEPDIR=$(DEPDIR) $(CXXDEPMODE)
$(depcomp) @AMDEPBACKSLASH@
address@hidden@ $(LTCXXCOMPILE) -c -o $@ $<
+
mostlyclean-libtool:
-rm -f *.lo
Index: src/five_point.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/five_point.cpp,v
retrieving revision 1.18
retrieving revision 1.19
diff -u -b -r1.18 -r1.19
--- src/five_point.cpp 11 May 2009 19:56:53 -0000 1.18
+++ src/five_point.cpp 15 May 2009 14:54:30 -0000 1.19
@@ -7,63 +7,15 @@
#include <TooN/SVD.h>
#include <algorithm>
+#include "polynomial.h"
using namespace TooN;
using namespace std;
using namespace std::tr1;
-extern "C" {
-#include "solve.h"
-}
namespace tag {
-vector<double> get_roots(const Vector<11> & p){
- // uses Graphics Gem I code
- // for details see here:
- // http://tog.acm.org/GraphicsGems/
-
- poly sseq[MAX_ORDER];
- Vector<11,double,Reference> v(sseq[0].coef);
- v = p;
-
- int num_poly = buildsturm(10, sseq);
- int atmin, atmax;
- int nroots = numroots(num_poly, sseq, &atmin, &atmax);
-
- vector<double> roots(nroots);
- if(nroots == 0)
- return roots;
-
- double min = -1.0;
- int nchanges = numchanges(num_poly, sseq, min);
- for (int i = 0; nchanges != atmin && i != MAXPOW; i++) {
- min *= 10.0;
- nchanges = numchanges(num_poly, sseq, min);
- }
-
- if (nchanges != atmin) {
- cerr << "solve: unable to bracket all negative roots" << endl;
- atmin = nchanges;
- }
-
- double max = 1.0;
- nchanges = numchanges(num_poly, sseq, max);
- for (int i = 0; nchanges != atmax && i != MAXPOW; i++) {
- max *= 10.0;
- nchanges = numchanges(num_poly, sseq, max);
- }
-
- if (nchanges != atmax) {
- cerr << "solve: unable to bracket all positive roots" << endl;
- atmax = nchanges;
- }
-
- nroots = atmin - atmax;
- sbisect(num_poly, sseq, min, max, atmin, atmax, &roots[0]);
- return roots;
-}
-
void build_matrix(const Vector<9>& X, const Vector<9>& Y, const Vector<9>& Z,
const Vector<9>& W, Matrix<10,20>& R);
Vector<9> stack_points(const Vector<3>&p, const Vector<3>& q)
@@ -114,6 +66,32 @@
return Matrix<3, 3, double, Reference::RowMajor>(&v[0]);
}
+
+// This function finds some vectors spanning the null space of m, assuming
+// that none of the rows of m are linearly dependent.
+//
+// m is a ahort, fat matrix.
+// The Gauss-Jordan decomposition is [I|A]
+//
+// [I|A] * [A^T|I]^T = [0]
+//
+// Since [I|A] spans the same space as m, then:
+//
+// m * [A^T|I]^T = [0]
+//
+// Therefore [A^T|I] spans the null space of m.
+template<int R, int C, class P> Matrix<C-R, C, P> dodgy_null(Matrix<R, C, P> m)
+{
+ gauss_jordan(m);
+
+ Matrix<C-R, C> null;
+ null.template slice<0,0,C-R, R>() = m.T().template slice<R,0,C-R, R>();
+ null.template slice<0,R, C-R, C-R>() = -Identity;
+
+ return null;
+}
+
+
vector<Matrix<3> > five_point(const array<pair<Vector<3>, Vector<3> >, 5> &
points)
{
//Equations numbers are given with reference to:
@@ -142,20 +120,17 @@
// linear sum of the remaining 4 null space vectors.
// See Eqn 10.
- Matrix<9, 9> Q = Zeros;
+ Matrix<5, 9> Q = Zeros;
for(int i=0; i < 5; i++)
Q[i] = stack_points(points[i].second, points[i].first);
- SVD<9, 9> svd_Q(Q);
+ Matrix<4,9> null = dodgy_null(Q);
- //The null-space it the last 4 rows of svd_Q.get_VT()
- //
// According to Eqn 10:
- Vector<9> X = svd_Q.get_VT()[5];
- Vector<9> Y = svd_Q.get_VT()[6];
- Vector<9> Z = svd_Q.get_VT()[7];
- Vector<9> W = svd_Q.get_VT()[8];
-
+ Vector<9> X = null[0];
+ Vector<9> Y = null[1];
+ Vector<9> Z = null[2];
+ Vector<9> W = null[3];
Matrix<10,20> R;
build_matrix(X, Y, Z, W, R);
@@ -226,7 +201,7 @@
//The polynomial is...
Vector<11> n = poly_mul(p1, b_31) + poly_mul(p2, b_32) + poly_mul(p3,
b_33);
- vector<double> roots = get_roots(n);
+ vector<double> roots = find_roots(n);
vector<Matrix<3> > Es;
for(unsigned i=0; i <roots.size(); i++)
@@ -363,10 +338,23 @@
return sq(point * line) / (sq(line[0]) + sq(line[1]));
}
+double point_line_distance(Vector<3> point, const Vector<3>& line)
+{
+ //Normalize the point to [x0, y0, 1]
+ point /= point[2];
+
+ return point * line / sqrt(sq(line[0]) + sq(line[1]));
+}
+
pair<double, double> essential_reprojection_errors_squared(const Matrix<3>& E,
const Vector<3>& q, const Vector<3>& p)
{
return make_pair(point_line_distance_squared(p, E*q),
point_line_distance_squared(q, E.T()*p));
}
+pair<double, double> essential_reprojection_errors(const Matrix<3>& E, const
Vector<3>& q, const Vector<3>& p)
+{
+ return make_pair(point_line_distance(p, E*q), point_line_distance(q,
E.T()*p));
+}
+
}
Index: tag/five_point.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/five_point.h,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- tag/five_point.h 11 May 2009 17:54:40 -0000 1.8
+++ tag/five_point.h 15 May 2009 14:54:31 -0000 1.9
@@ -54,7 +54,8 @@
/// functions computes the reprojection errors given by the squared distance
from
/// \e p to the line defined by \f$ E\vec{q} \f$ and the squared distance from
/// \e q to the line defined by \f$ E^T\vec{p} \f$. If \e E is not an
essential matrix
-/// then the errors will not be sensible.
+/// then the errors will not be sensible. This function avoids a sqrt compared
to
+/// tag::essential_reprojection_errors.
///
///@param E \e E: essential matrix
///@param q \e q: right hand (first) point
@@ -62,6 +63,19 @@
///@returns the reprojection errors
/// @ingroup essentialgroup
std::pair<double, double> essential_reprojection_errors_squared(const
TooN::Matrix<3>& E, const TooN::Vector<3>&q, const TooN::Vector<3>& p);
+
+/// Given an essential matrix \e E and two points \e p and \e q, this
+/// functions computes the reprojection errors given by the signed distance
from
+/// \e p to the line defined by \f$ E\vec{q} \f$ and the signed distance from
+/// \e q to the line defined by \f$ E^T\vec{p} \f$. If \e E is not an
essential matrix
+/// then the errors will not be sensible.
+///
+///@param E \e E: essential matrix
+///@param q \e q: right hand (first) point
+///@param p \e p: left hand (second) point
+///@returns the reprojection errors
+/// @ingroup essentialgroup
+std::pair<double, double> essential_reprojection_errors(const TooN::Matrix<3>&
E, const TooN::Vector<3>&q, const TooN::Vector<3>& p);
}
#endif // TAG_FIVE_POINT
Index: src/polynomial.cc
===================================================================
RCS file: src/polynomial.cc
diff -N src/polynomial.cc
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ src/polynomial.cc 15 May 2009 14:54:30 -0000 1.1
@@ -0,0 +1,402 @@
+#include <TooN/TooN.h>
+#include <utility>
+#include <vector>
+#include <tr1/tuple>
+#include <tr1/array>
+
+#include "polynomial.h"
+
+using namespace TooN;
+using namespace std;
+using namespace std::tr1;
+
+
+template<int AN, int BN> Vector<AN+BN-1> poly_mul(const Vector<AN>& a, const
Vector<BN>& b)
+{
+ //Polynomials are stored with the coefficient of zero in the first
+
+ Vector<AN+BN-1> ret = Zeros;
+
+ for(int ai=0; ai < a.size(); ai++)
+ for(int bi=0; bi <b.size(); bi++)
+ ret[ai+bi] += a[ai] * b[bi];
+
+ return ret;
+}
+template<int N> Vector<N-1> polydiff(const Vector<N>& v)
+{
+ Vector<N-1> ret;
+
+ for(int i=1; i < N; i++)
+ ret[i-1] = v[i] * i;
+
+ return ret;
+}
+
+template<int NumNum, int NumDenom>
+pair<Vector<NumNum-NumDenom+1>, Vector<NumDenom-1> > poly_div(Vector<NumNum>
num, const Vector<NumDenom>& denom)
+{
+ static const int NumQuot = NumNum-NumDenom+1;
+ static const int NumRem = NumDenom - 1;
+
+ Vector<NumQuot> quotient;
+
+ for(int i=NumQuot-1; i >=0 ; i--)
+ {
+ double scale = num[i+denom.size()-1] / denom[denom.size()-1];
+ quotient[i] = scale;
+
+ for(int j=0; j < denom.size(); j++)
+ num[i+j] -= scale * denom[j];
+ }
+
+ Vector<NumRem> remainder = num.template slice<0,NumRem>();
+ return make_pair(quotient, remainder);
+}
+
+template<int N, int M>
+pair<Vector<N>, Vector<M> > neg_second(const pair<Vector<N>, Vector<M> >& r)
+{
+ return make_pair(r.first, -r.second);
+}
+
+struct SturmChain10
+{
+ Vector<11> f10;
+ Vector<10> f9;
+ Vector<9 > f8;
+ Vector<8 > f7;
+ Vector<7 > f6;
+ Vector<6 > f5;
+ Vector<5 > f4;
+ Vector<4 > f3;
+ Vector<3 > f2;
+ Vector<2 > f1;
+ Vector<1 > f0;
+
+ array<Vector<2>, 11> q;
+
+ SturmChain10(const Vector<11>& p)
+ {
+ f10 = p;
+ f9 = polydiff(p);
+
+ tie(q[10], f8) = neg_second(poly_div(f10, f9));
+ tie(q[ 9], f7) = neg_second(poly_div(f9, f8));
+ tie(q[ 8], f6) = neg_second(poly_div(f8, f7));
+ tie(q[ 7], f5) = neg_second(poly_div(f7, f6));
+ tie(q[ 6], f4) = neg_second(poly_div(f6, f5));
+ tie(q[ 5], f3) = neg_second(poly_div(f5, f4));
+ tie(q[ 4], f2) = neg_second(poly_div(f4, f3));
+ tie(q[ 3], f1) = neg_second(poly_div(f3, f2));
+ tie(q[ 2], f0) = neg_second(poly_div(f2, f1));
+ }
+
+ // The sturm chain is constructed so that:
+ //
+ // f_n = P
+ // f_n-1 = P'
+ // f_n-2 = -REM(f_n, f_n-1)
+ // ...
+ //
+ // Simply from division:
+ //
+ // f_i = QUOT(f_i, f_i-1) * f_i-1 + REM(f_i, r_i-1)
+ //
+ // From the sequence, then:
+ //
+ // f_i = Q_i * f_i-1 - f_i-2
+ //
+ // Q_i is always of degree 1, so the number of sign changes
+ // can be evaluated using the recursion above efficiently.
+ pair<int, double> changes(double x) const
+ {
+ int changes=0;
+ double v_prev_2 = polyval(f0, x);
+ double v_prev_1 = polyval(f1, x);
+
+ if(signbit(v_prev_1) != signbit(v_prev_2))
+ changes++;
+
+ for(int i=2; i < 11; i++)
+ {
+ double v = polyval(q[i], x) * v_prev_1 - v_prev_2;
+
+ if(signbit(v) != signbit(v_prev_1))
+ changes++;
+ v_prev_2 = v_prev_1;
+ v_prev_1 = v;
+ }
+ return make_pair(changes, v_prev_1);
+ }
+
+ // At infinity, all aditive terms disappear
+ int changes_at_infinity() const
+ {
+ int signs = 0;
+
+ bool s_0 = signbit(f0[0]);
+ bool s_prev = signbit(f1[1]);
+
+ if(s_0 != s_prev)
+ signs++;
+
+ for(int i=2; i < 11; i++)
+ {
+ bool s = s_prev != (bool)signbit(q[i][1]);
+
+ if(s != s_prev)
+ signs++;
+
+ s_prev = s;
+ }
+ return signs;
+ }
+
+ int changes_at_neg_infinity() const
+ {
+ int signs = 0;
+
+ bool s_0 = signbit(f0[0]);
+ bool s_prev = !signbit(f1[1]);
+
+ if(s_0 != s_prev)
+ signs++;
+
+ for(int i=2; i < 11; i++)
+ {
+ bool s = s_prev != (bool)!signbit(q[i][1]);
+
+ if(s != s_prev)
+ signs++;
+
+ s_prev = s;
+ }
+ return signs;
+ }
+
+ double operator()(double x) const
+ {
+ return polyval(f10, x);
+ }
+
+ double deriv(double x) const
+ {
+ return polyval(f9, x);
+ }
+};
+
+template<class F> double polish_root_bisection(double lower, double upper,
double lower_val, double upper_val, const F& f)
+{
+ const double zeps = 1e-20;
+ const double eps = 1e-10;
+ static const int maxits = 50;
+
+ int it;
+
+ for(it = 0; fabs(upper-lower) / (fabs(lower) + fabs(upper)) > eps &&
fabs(upper-lower) > zeps && it < maxits; it++)
+ {
+ double midpoint = (upper + lower)/2;
+ double mid_val = f(midpoint);
+
+ if((bool)signbit(mid_val) == (bool)signbit(upper_val))
+ {
+ upper_val = mid_val;
+ upper = midpoint;
+ }
+ else
+ {
+ lower_val = mid_val;
+ lower = midpoint;
+ }
+ }
+
+ cerr << "Converged in " << it << " iterations\n";
+
+ return (upper + lower)/2;
+}
+
+template<class F> pair<double, double> polish_root_newton(double lower, double
upper, double lower_val, double upper_val, const F& f)
+{
+ const double zeps = 1e-20;
+ const double eps = 1e-15;
+ static const int maxits = 10;
+
+ double r = (lower + upper)/2; //Initial root guess
+ double fr = f(r);
+ double fdr = f.deriv(r);
+
+ double dxold = upper-lower;
+ double dx = dxold;
+
+ int it;
+
+ for(it=0; it < maxits; it++)
+ {
+ double newton_step = -fr / fdr;
+
+ //Check suitability of newton step.
+ //Must be within bounds and reasonably quick convergence
+ if(r + newton_step > upper || r + newton_step < lower ||
2*fabs(newton_step) > fabs(dxold))
+ {
+ //Unsuitable, so take a bisection step
+ dxold = dx;
+ dx = (upper - lower) / 2;
+ r = lower + dx; //r = (xl+xh)/2
+ }
+ else
+ {
+ r += newton_step;
+
+ dxold = dx;
+ dx = newton_step;
+ }
+
+ //Check for convergence
+ if( fabs(dx / r) < eps || fabs(dx) < zeps)
+ return make_pair(r, fr);
+
+ //Update
+ fr = f(r);
+ fdr = f.deriv(r);
+
+ if( (bool)signbit(fr) == (bool)signbit(upper_val))
+ {
+ upper_val = fr;
+ upper = r;
+ }
+ else
+ {
+ lower_val = fr;
+ lower = r;
+ }
+ }
+
+ return make_pair(r, fr);
+}
+
+
+
+
+vector<double> find_roots(const Vector<11>& v)
+{
+ //This is a coarse root-finding routine. No effort is made to
+ //track down really reluctant roots, they are simply discarded.
+ //Easy roots are found to a decent accuracy.
+
+ const double zeps = 1e-20;
+ const double eps = 1e-10;
+ const double feps = 1e-4; //Epsilon for the function value
+
+ //Roots above this are impossible to determine accurately
+ //since the polynomial is so steep that even machine precision
+ //errors in the root can lead to vast errors.
+ const double highest_root = 100;
+
+ SturmChain10 s(v);
+
+ vector<double> roots;
+
+ int at_n_inf = s.changes_at_neg_infinity();
+ int at_p_inf = s.changes_at_infinity();
+
+ int num_roots = at_n_inf - at_p_inf;
+
+ //Try to find an upper bound for the highest root
+ double upper=1, upper_val;
+
+ for(;;)
+ {
+ int c;
+ tie(c, upper_val) = s.changes(upper);
+
+
+ if(at_n_inf - c < num_roots && upper < highest_root)
+ upper*=2;
+ else
+ break;
+ }
+
+ //Try to find a lower bound for the lowest roots
+ double lower=-1, lower_val;
+ for(;;)
+ {
+ int c;
+ tie(c, lower_val) = s.changes(lower);
+
+ if(c - at_p_inf < num_roots && lower > -highest_root)
+ lower*=2;
+ else
+ break;
+ }
+
+
+ //Now we have bounds for all roots and some assosciated polynomial
+ //values.
+
+
+ vector<tuple<double, double, int, int, double, double> > intervals;
+ intervals.push_back(make_tuple(lower, upper, at_n_inf, at_p_inf,
lower_val, upper_val));
+
+ while(!intervals.empty())
+ {
+ int at_lower, at_upper;
+
+ tie(lower, upper, at_lower, at_upper, lower_val, upper_val) =
intervals.back();
+ intervals.pop_back();
+
+ double midpoint = (upper + lower)/2;
+
+ //If the bracket is good, then continue, otherwise discard it.
+ if(fabs((upper-lower) / midpoint) > eps && fabs(upper - lower)
> zeps)
+ {
+ int at_midpoint;
+ double midpoint_val;
+
+ tie(at_midpoint, midpoint_val) = s.changes(midpoint);
+
+
+ if(at_midpoint == at_lower)
+ intervals.push_back(make_tuple(midpoint, upper,
at_midpoint, at_upper, midpoint_val, upper_val));
+ else if(at_midpoint == at_upper)
+ intervals.push_back(make_tuple(lower, midpoint,
at_lower, at_midpoint, lower_val, midpoint_val));
+ else
+ {
+ //We've split the interval
+
+ //Check to see if the lower interval has
bracketed a single
+ //root
+ if(at_lower - at_midpoint == 1)
+ {
+ double root, rootval;
+
+ tie(root, rootval) =
polish_root_newton(lower, midpoint, lower_val, midpoint_val, s);
+
+ //Discard poor-quality roots
+ if(fabs(rootval) < feps)
+ roots.push_back(root);
+ }
+ else
+ intervals.push_back(make_tuple(lower,
midpoint, at_lower, at_midpoint, lower_val, midpoint_val));
+
+ //Do the same with the upper interval
+ if(at_midpoint - at_upper == 1)
+ {
+ double root, rootval;
+
+ tie(root, rootval) =
polish_root_newton(midpoint, upper, midpoint_val, upper_val, s);
+
+ //Discard poor-quality roots
+ if(fabs(rootval) < feps)
+ roots.push_back(root);
+ }
+ else
+
intervals.push_back(make_tuple(midpoint, upper, at_midpoint, at_upper,
midpoint_val, upper_val));
+ }
+ }
+ }
+
+ return roots;
+}
+
+
Index: src/polynomial.h
===================================================================
RCS file: src/polynomial.h
diff -N src/polynomial.h
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ src/polynomial.h 15 May 2009 14:54:30 -0000 1.1
@@ -0,0 +1,23 @@
+#ifndef POLYNOMIAL_H
+#define POLYNOMIAL_H
+#include <vector>
+#include <TooN/TooN.h>
+
+std::vector<double> find_roots(const TooN::Vector<11>& v);
+
+
+template<int N> double polyval(const TooN::Vector<N>& v, double x)
+{
+ //Polynomials are stored with the coefficient of zero in the first
+ double val=0;
+ for(int i=v.size()-1; i > 0; i--)
+ {
+ val += v[i];
+ val *= x;
+ }
+
+ val += v[0];
+ return val;
+}
+
+#endif
Index: src/solve.h
===================================================================
RCS file: src/solve.h
diff -N src/solve.h
--- src/solve.h 22 Apr 2009 23:23:12 -0000 1.1
+++ /dev/null 1 Jan 1970 00:00:00 -0000
@@ -1,41 +0,0 @@
-
-/*
- * solve.h
- *
- * some useful constants and types.
- */
-#define MAX_ORDER 12
-/* maximum order for a polynomial */
-
-#define RELERROR 1.0e-14
-/* smallest relative error we want */
-
-#define MAXPOW 32
-/* max power of 10 we wish to search to */
-
-#define MAXIT 800
-/* max number of iterations */
-
-/* a coefficient smaller than SMALL_ENOUGH is considered to
- be zero (0.0). */
-
-#define SMALL_ENOUGH 1.0e-12
-
-
-/*
- * structure type for representing a polynomial
- */
-typedef struct p {
- int ord;
- double coef[MAX_ORDER+1];
-} poly;
-
-int modrf();
-int numroots(int np, poly * sseq, int * atneg, int * atpos);
-int numchanges(int np, poly * sseq, double a);
-int buildsturm(int ord, poly *sseq);
-
-double evalpoly();
-
-void sbisect(int np, poly * sseq, double min, double max, int atmin, int
atmax,double * roots);
-
Index: src/sturm.c
===================================================================
RCS file: src/sturm.c
diff -N src/sturm.c
--- src/sturm.c 23 Apr 2009 15:07:38 -0000 1.2
+++ /dev/null 1 Jan 1970 00:00:00 -0000
@@ -1,287 +0,0 @@
-
-/*
- * sturm.c
- *
- * the functions to build and evaluate the Sturm sequence
- */
-#include <math.h>
-#include <stdio.h>
-#include "solve.h"
-
-/*
- * modp
- *
- * calculates the modulus of u(x) / v(x) leaving it in r, it
- * returns 0 if r(x) is a constant.
- * note: this function assumes the leading coefficient of v
- * is 1 or -1
- */
-static int
-modp(u, v, r)
- poly *u, *v, *r;
-{
- int k, j;
- double *nr, *end, *uc;
-
- nr = r->coef;
- end = &u->coef[u->ord];
-
- uc = u->coef;
- while (uc <= end)
- *nr++ = *uc++;
-
- if (v->coef[v->ord] < 0.0) {
-
-
- for (k = u->ord - v->ord - 1; k >= 0; k -= 2)
- r->coef[k] = -r->coef[k];
-
- for (k = u->ord - v->ord; k >= 0; k--)
- for (j = v->ord + k - 1; j >= k; j--)
- r->coef[j] = -r->coef[j] -
r->coef[v->ord + k]
- * v->coef[j - k];
- } else {
- for (k = u->ord - v->ord; k >= 0; k--)
- for (j = v->ord + k - 1; j >= k; j--)
- r->coef[j] -= r->coef[v->ord + k] * v->coef[j -
k];
- }
-
- k = v->ord - 1;
- while (k >= 0 && fabs(r->coef[k]) < SMALL_ENOUGH) {
- r->coef[k] = 0.0;
- k--;
- }
-
- r->ord = (k < 0) ? 0 : k;
-
- return(r->ord);
-}
-
-/*
- * buildsturm
- *
- * build up a sturm sequence for a polynomial in smat, returning
- * the number of polynomials in the sequence
- */
-int
-buildsturm(ord, sseq)
- int ord;
- poly *sseq;
-{
- int i;
- double f, *fp, *fc;
- poly *sp;
-
- sseq[0].ord = ord;
- sseq[1].ord = ord - 1;
-
-
- /*
- * calculate the derivative and normalise the leading
- * coefficient.
- */
- f = fabs(sseq[0].coef[ord] * ord);
- fp = sseq[1].coef;
- fc = sseq[0].coef + 1;
- for (i = 1; i <= ord; i++)
- *fp++ = *fc++ * i / f;
-
- /*
- * construct the rest of the Sturm sequence
- */
- for (sp = sseq + 2; modp(sp - 2, sp - 1, sp); sp++) {
-
- /*
- * reverse the sign and normalise
- */
- f = -fabs(sp->coef[sp->ord]);
- for (fp = &sp->coef[sp->ord]; fp >= sp->coef; fp--)
- *fp /= f;
- }
-
- sp->coef[0] = -sp->coef[0]; /* reverse the sign */
-
- return(sp - sseq);
-}
-
-/*
- * numroots
- *
- * return the number of distinct real roots of the polynomial
- * described in sseq.
- */
-int
-numroots(np, sseq, atneg, atpos)
- int np;
- poly *sseq;
- int *atneg, *atpos;
-{
- int atposinf, atneginf;
- poly *s;
- double f, lf;
-
- atposinf = atneginf = 0;
-
-
- /*
- * changes at positive infinity
- */
- lf = sseq[0].coef[sseq[0].ord];
-
- for (s = sseq + 1; s <= sseq + np; s++) {
- f = s->coef[s->ord];
- if (lf == 0.0 || lf * f < 0)
- atposinf++;
- lf = f;
- }
-
- /*
- * changes at negative infinity
- */
- if (sseq[0].ord & 1)
- lf = -sseq[0].coef[sseq[0].ord];
- else
- lf = sseq[0].coef[sseq[0].ord];
-
- for (s = sseq + 1; s <= sseq + np; s++) {
- if (s->ord & 1)
- f = -s->coef[s->ord];
- else
- f = s->coef[s->ord];
- if (lf == 0.0 || lf * f < 0)
- atneginf++;
- lf = f;
- }
-
- *atneg = atneginf;
- *atpos = atposinf;
-
- return(atneginf - atposinf);
-}
-
-/*
- * numchanges
- *
- * return the number of sign changes in the Sturm sequence in
- * sseq at the value a.
- */
-int
-numchanges(np, sseq, a)
- int np;
- poly *sseq;
- double a;
-
-{
- int changes;
- double f, lf;
- poly *s;
-
- changes = 0;
-
- lf = evalpoly(sseq[0].ord, sseq[0].coef, a);
-
- for (s = sseq + 1; s <= sseq + np; s++) {
- f = evalpoly(s->ord, s->coef, a);
- if (lf == 0.0 || lf * f < 0)
- changes++;
- lf = f;
- }
-
- return(changes);
-}
-
-/*
- * sbisect
- *
- * uses a bisection based on the sturm sequence for the polynomial
- * described in sseq to isolate intervals in which roots occur,
- * the roots are returned in the roots array in order of magnitude.
- */
-void sbisect(np, sseq, min, max, atmin, atmax, roots)
- int np;
- poly *sseq;
- double min, max;
- int atmin, atmax;
- double *roots;
-{
- double mid;
- int n1 = 0, n2 = 0, its, atmid, nroot;
-
- if ((nroot = atmin - atmax) == 1) {
-
- /*
- * first try a less expensive technique.
- */
- if (modrf(sseq->ord, sseq->coef, min, max, &roots[0]))
- return;
-
-
- /*
- * if we get here we have to evaluate the root the hard
- * way by using the Sturm sequence.
- */
- for (its = 0; its < MAXIT; its++) {
- mid = (min + max) / 2;
-
- atmid = numchanges(np, sseq, mid);
-
- if (fabs(mid) > RELERROR) {
- if (fabs((max - min) / mid) < RELERROR)
{
- roots[0] = mid;
- return;
- }
- } else if (fabs(max - min) < RELERROR) {
- roots[0] = mid;
- return;
- }
-
- if ((atmin - atmid) == 0)
- min = mid;
- else
- max = mid;
- }
-
- if (its == MAXIT) {
- fprintf(stderr, "sbisect: overflow min %f max
%f\
- diff %e nroot %d n1 %d n2 %d\n",
- min, max, max - min, nroot, n1, n2);
- roots[0] = mid;
- }
-
- return;
- }
-
- /*
- * more than one root in the interval, we have to bisect...
- */
- for (its = 0; its < MAXIT; its++) {
-
- mid = (min + max) / 2;
-
- atmid = numchanges(np, sseq, mid);
-
- n1 = atmin - atmid;
- n2 = atmid - atmax;
-
-
- if (n1 != 0 && n2 != 0) {
- sbisect(np, sseq, min, mid, atmin, atmid,
roots);
- sbisect(np, sseq, mid, max, atmid, atmax,
&roots[n1]);
- break;
- }
-
- if (n1 == 0)
- min = mid;
- else
- max = mid;
- }
-
- if (its == MAXIT) {
- fprintf(stderr, "sbisect: roots too close together\n");
- fprintf(stderr, "sbisect: overflow min %f max %f diff
%e\
- nroot %d n1 %d n2 %d\n",
- min, max, max - min, nroot, n1, n2);
- for (n1 = atmax; n1 < atmin; n1++)
- roots[n1 - atmax] = mid;
- }
-}
Index: src/util.c
===================================================================
RCS file: src/util.c
diff -N src/util.c
--- src/util.c 23 Apr 2009 17:20:27 -0000 1.2
+++ /dev/null 1 Jan 1970 00:00:00 -0000
@@ -1,119 +0,0 @@
-
-/*
- * util.c
- *
- * some utlity functions for root polishing and evaluating
- * polynomials.
- */
-#include <math.h>
-#include <stdio.h>
-#include "solve.h"
-
-/*
- * evalpoly
- *
- * evaluate polynomial defined in coef returning its value.
- */
-inline double evalpoly (ord, coef, x)
- int ord;
- double *coef, x;
-{
- double *fp, f;
-
- fp = &coef[ord];
- f = *fp;
-
- for (fp--; fp >= coef; fp--)
- f = x * f + *fp;
-
- return(f);
-}
-
-
-/*
- * modrf
- *
- * uses the modified regula-falsi method to evaluate the root
- * in interval [a,b] of the polynomial described in coef. The
- * root is returned is returned in *val. The routine returns zero
- * if it can't converge.
- */
-int
-modrf(ord, coef, a, b, val)
- int ord;
- double *coef;
- double a, b, *val;
-{
- int its;
- double fa, fb, x, fx, lfx;
- double *fp, *scoef, *ecoef;
-
- scoef = coef;
- ecoef = &coef[ord];
-
- fb = fa = *ecoef;
- for (fp = ecoef - 1; fp >= scoef; fp--) {
- fa = a * fa + *fp;
- fb = b * fb + *fp;
- }
-
- /*
- * if there is no sign difference the method won't work
- */
- if (fa * fb > 0.0)
- return(0);
-
- if (fabs(fa) < RELERROR) {
- *val = a;
- return(1);
- }
-
- if (fabs(fb) < RELERROR) {
- *val = b;
- return(1);
- }
-
- lfx = fa;
-
-
- for (its = 0; its < MAXIT; its++) {
-
- x = (fb * a - fa * b) / (fb - fa);
-
- fx = *ecoef;
- for (fp = ecoef - 1; fp >= scoef; fp--)
- fx = x * fx + *fp;
-
- if (fabs(x) > RELERROR) {
- if (fabs(fx / x) < RELERROR) {
- *val = x;
- return(1);
- }
- } else if (fabs(fx) < RELERROR) {
- *val = x;
- return(1);
- }
- if(fabs(a-b)/(fabs(a)+1e-20) < RELERROR){
- *val = x;
- return(0);
- }
-
- if ((fa * fx) < 0) {
- b = x;
- fb = fx;
- if ((lfx * fx) > 0)
- fa /= 2;
- } else {
- a = x;
- fa = fx;
- if ((lfx * fx) > 0)
- fb /= 2;
- }
-
- lfx = fx;
- }
-
- fprintf(stderr, "\nmodrf overflow %20.17e %20.17e %20.17e %20.17e
%20.17e %20.17e %20.17e\n", a, b, a-b, fa, fb, fx, fabs(a-b)/(fabs(a)+1e-20));
-
- return(0);
-}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag Makefile.am Makefile.in src/five_point.cpp ...,
Edward Rosten <=