Index: ThirdParty/levmar/src/Axb.c
===================================================================
--- ThirdParty/levmar/src/Axb.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/Axb.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,75 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Solution of linear systems involved in the Levenberg - Marquardt
+//  minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * LAPACK-based implementations for various linear system solvers. The same core
+ * code is used with appropriate #defines to derive single and double precision
+ * solver versions, see also Axb_core.c
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "levmar.h"
+#include "misc.h"
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+#define LM_CNST(x) (x)
+#ifndef HAVE_LAPACK
+#include <float.h>
+#define LM_REAL_EPSILON DBL_EPSILON
+#endif
+
+#include "Axb_core.c"
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_CNST
+#undef LM_REAL_EPSILON
+#endif /* LM_DBL_PREC */
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+#ifndef HAVE_LAPACK
+#define LM_REAL_EPSILON FLT_EPSILON
+#endif
+
+#include "Axb_core.c"
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef __SUBCNST
+#undef LM_CNST
+#undef LM_REAL_EPSILON
+#endif /* LM_SNGL_PREC */
Index: ThirdParty/levmar/src/Axb_core.c
===================================================================
--- ThirdParty/levmar/src/Axb_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/Axb_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,1282 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Solution of linear systems involved in the Levenberg - Marquardt
+//  minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+
+/* Solvers for the linear systems Ax=b. Solvers should NOT modify their A & B arguments! */
+
+
+#ifndef LM_REAL // not included by Axb.c
+#error This file should not be compiled directly!
+#endif
+
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+#define __STATIC__ static
+#else
+#define __STATIC__ // empty
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+#ifdef HAVE_LAPACK
+
+/* prototypes of LAPACK routines */
+
+#define GEQRF LM_MK_LAPACK_NAME(geqrf)
+#define ORGQR LM_MK_LAPACK_NAME(orgqr)
+#define TRTRS LM_MK_LAPACK_NAME(trtrs)
+#define POTF2 LM_MK_LAPACK_NAME(potf2)
+#define POTRF LM_MK_LAPACK_NAME(potrf)
+#define POTRS LM_MK_LAPACK_NAME(potrs)
+#define GETRF LM_MK_LAPACK_NAME(getrf)
+#define GETRS LM_MK_LAPACK_NAME(getrs)
+#define GESVD LM_MK_LAPACK_NAME(gesvd)
+#define GESDD LM_MK_LAPACK_NAME(gesdd)
+#define SYTRF LM_MK_LAPACK_NAME(sytrf)
+#define SYTRS LM_MK_LAPACK_NAME(sytrs)
+#define PLASMA_POSV LM_CAT_(PLASMA_, LM_ADD_PREFIX(posv))
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/* QR decomposition */
+extern int GEQRF(int *m, int *n, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
+extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
+
+/* solution of triangular systems */
+extern int TRTRS(char *uplo, char *trans, char *diag, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);
+
+/* Cholesky decomposition and systems solution */
+extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);
+extern int POTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *info); /* block version of dpotf2 */
+extern int POTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);
+
+/* LU decomposition and systems solution */
+extern int GETRF(int *m, int *n, LM_REAL *a, int *lda, int *ipiv, int *info);
+extern int GETRS(char *trans, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);
+
+/* Singular Value Decomposition (SVD) */
+extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,
+                   LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);
+
+/* lapack 3.0 new SVD routine, faster than xgesvd().
+ * In case that your version of LAPACK does not include them, use the above two older routines
+ */
+extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,
+                   LM_REAL *work, int *lwork, int *iwork, int *info);
+
+/* LDLt/UDUt factorization and systems solution */
+extern int SYTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *ipiv, LM_REAL *work, int *lwork, int *info);
+extern int SYTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);
+#ifdef __cplusplus
+}
+#endif
+
+/* precision-specific definitions */
+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
+#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
+#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function is based on QR decomposition with explicit computation of Q:
+ * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes
+ * Q R x = b or R x = Q^T b.
+ * The last equation can be solved directly.
+ *
+ * A is mxm, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_QR(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+static int nb=0; /* no __STATIC__ decl. here! */
+
+LM_REAL *a, *tau, *r, *work;
+int a_sz, tau_sz, r_sz, tot_sz;
+register int i, j;
+int info, worksz, nrhs=1;
+register LM_REAL sum;
+
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+      return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+    /* calculate required memory size */
+    a_sz=m*m;
+    tau_sz=m;
+    r_sz=m*m; /* only the upper triangular part really needed */
+    if(!nb){
+      LM_REAL tmp;
+
+      worksz=-1; // workspace query; optimal size is returned in tmp
+      GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
+      nb=((int)tmp)/m; // optimal worksize is m*nb
+    }
+    worksz=nb*m;
+    tot_sz=a_sz + tau_sz + r_sz + worksz;
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+      if(buf) free(buf); /* free previously allocated memory */
+
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
+        exit(1);
+      }
+    }
+#else
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
+        exit(1);
+      }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+    a=buf;
+    tau=a+a_sz;
+    r=tau+tau_sz;
+    work=r+r_sz;
+
+  /* store A (column major!) into a */
+	for(i=0; i<m; i++)
+		for(j=0; j<m; j++)
+			a[i+j*m]=A[i*m+j];
+
+  /* QR decomposition of A */
+  GEQRF((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QR) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QR) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* R is stored in the upper triangular part of a; copy it in r so that ORGQR() below won't destroy it */ 
+  memcpy(r, a, r_sz*sizeof(LM_REAL));
+
+  /* compute Q using the elementary reflectors computed by the above decomposition */
+  ORGQR((int *)&m, (int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", AX_EQ_B_QR) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("Unknown LAPACK error (%d) in ", AX_EQ_B_QR) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* Q is now in a; compute Q^T b in x */
+  for(i=0; i<m; i++){
+    for(j=0, sum=0.0; j<m; j++)
+      sum+=a[i*m+j]*B[j];
+    x[i]=sum;
+  }
+
+  /* solve the linear system R x = Q^t b */
+  TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, r, (int *)&m, x, (int *)&m, &info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QR) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QR) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+/*
+ * This function returns the solution of min_x ||Ax - b||
+ *
+ * || . || is the second order (i.e. L2) norm. This is a least squares technique that
+ * is based on QR decomposition:
+ * If A=Q R with Q orthogonal and R upper triangular, the normal equations become
+ * (A^T A) x = A^T b  or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.
+ * This amounts to solving R^T y = A^T b for y and then R x = y for x
+ * Note that Q does not need to be explicitly computed
+ *
+ * A is mxn, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_QRLS(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m, int n)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+static int nb=0; /* no __STATIC__ decl. here! */
+
+LM_REAL *a, *tau, *r, *work;
+int a_sz, tau_sz, r_sz, tot_sz;
+register int i, j;
+int info, worksz, nrhs=1;
+register LM_REAL sum;
+   
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+      return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+    if(m<n){
+		  fprintf(stderr, RCAT("Normal equations require that the number of rows is greater than number of columns in ", AX_EQ_B_QRLS) "() [%d x %d]! -- try transposing\n", m, n);
+		  exit(1);
+	  }
+      
+    /* calculate required memory size */
+    a_sz=m*n;
+    tau_sz=n;
+    r_sz=n*n;
+    if(!nb){
+      LM_REAL tmp;
+
+      worksz=-1; // workspace query; optimal size is returned in tmp
+      GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
+      nb=((int)tmp)/m; // optimal worksize is m*nb
+    }
+    worksz=nb*m;
+    tot_sz=a_sz + tau_sz + r_sz + worksz;
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+      if(buf) free(buf); /* free previously allocated memory */
+
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
+        exit(1);
+      }
+    }
+#else
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
+        exit(1);
+      }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+    a=buf;
+    tau=a+a_sz;
+    r=tau+tau_sz;
+    work=r+r_sz;
+
+  /* store A (column major!) into a */
+	for(i=0; i<m; i++)
+		for(j=0; j<n; j++)
+			a[i+j*m]=A[i*n+j];
+
+  /* compute A^T b in x */
+  for(i=0; i<n; i++){
+    for(j=0, sum=0.0; j<m; j++)
+      sum+=A[j*n+i]*B[j];
+    x[i]=sum;
+  }
+
+  /* QR decomposition of A */
+  GEQRF((int *)&m, (int *)&n, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* R is stored in the upper triangular part of a. Note that a is mxn while r nxn */
+  for(j=0; j<n; j++){
+    for(i=0; i<=j; i++)
+      r[i+j*n]=a[i+j*m];
+
+    /* lower part is zero */
+    for(i=j+1; i<n; i++)
+      r[i+j*n]=0.0;
+  }
+
+  /* solve the linear system R^T y = A^t b */
+  TRTRS("U", "T", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* solve the linear system R x = y */
+  TRTRS("U", "N", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+/*
+ * This function returns the solution of Ax=b
+ *
+ * The function assumes that A is symmetric & postive definite and employs
+ * the Cholesky decomposition:
+ * If A=L L^T with L lower triangular, the system to be solved becomes
+ * (L L^T) x = b
+ * This amounts to solving L y = b for y and then L^T x = y for x
+ *
+ * A is mxm, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+LM_REAL *a;
+int a_sz, tot_sz;
+int info, nrhs=1;
+   
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+      return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+    /* calculate required memory size */
+    a_sz=m*m;
+    tot_sz=a_sz;
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+      if(buf) free(buf); /* free previously allocated memory */
+
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
+        exit(1);
+      }
+    }
+#else
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
+        exit(1);
+      }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+  a=buf;
+
+  /* store A into a and B into x. A is assumed symmetric,
+   * hence no transposition is needed
+   */
+  memcpy(a, A, a_sz*sizeof(LM_REAL));
+  memcpy(x, B, m*sizeof(LM_REAL));
+
+  /* Cholesky decomposition of A */
+  //POTF2("L", (int *)&m, a, (int *)&m, (int *)&info);
+  POTRF("L", (int *)&m, a, (int *)&m, (int *)&info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTF2) "/", POTRF) " in ",
+                      AX_EQ_B_CHOL) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for ", POTF2) "/", POTRF) " in ", AX_EQ_B_CHOL) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* solve using the computed Cholesky in one lapack call */
+  POTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+  if(info<0){
+    fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
+    exit(1);
+  }
+
+#if 0
+  /* alternative: solve the linear system L y = b ... */
+  TRTRS("L", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  /* ... solve the linear system L^T x = y */
+  TRTRS("L", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) "in ", AX_EQ_B_CHOL) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+#endif /* 0 */
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+#ifdef HAVE_PLASMA
+
+/* Linear algebra using PLASMA parallel library for multicore CPUs.
+ * http://icl.cs.utk.edu/plasma/
+ *
+ * WARNING: BLAS multithreading should be disabled, e.g. setenv MKL_NUM_THREADS 1
+ */
+
+#ifndef _LM_PLASMA_MISC_
+/* avoid multiple inclusion of helper code */
+#define _LM_PLASMA_MISC_
+
+#include <plasma.h>
+#include <cblas.h>
+#include <lapacke.h>
+#include <plasma_tmg.h>
+#include <core_blas.h>
+
+/* programmatically determine the number of cores on the current machine */
+#ifdef _WIN32
+#include <windows.h>
+#elif __linux
+#include <unistd.h>
+#endif
+static int getnbcores()
+{
+#ifdef _WIN32
+  SYSTEM_INFO sysinfo;
+  GetSystemInfo(&sysinfo);
+  return sysinfo.dwNumberOfProcessors;
+#elif __linux
+  return sysconf(_SC_NPROCESSORS_ONLN);
+#else // unknown system
+  return 2<<1; // will be halved by right shift below
+#endif
+}
+
+static int PLASMA_ncores=-(getnbcores()>>1); // >0 if PLASMA initialized, <0 otherwise
+
+/* user-specified number of cores */
+void levmar_PLASMA_setnbcores(int cores)
+{
+  PLASMA_ncores=(cores>0)? -cores : ((cores)? cores : -2);
+}
+#endif /* _LM_PLASMA_MISC_ */
+
+/*
+ * This function returns the solution of Ax=b
+ *
+ * The function assumes that A is symmetric & positive definite and employs the
+ * Cholesky decomposition implemented by PLASMA for homogeneous multicore processors.
+ *
+ * A is mxm, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_PLASMA_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+LM_REAL *a;
+int a_sz, tot_sz;
+int info, nrhs=1;
+
+    if(A==NULL){
+#ifdef LINSOLVERS_RETAIN_MEMORY
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+      PLASMA_Finalize();
+      PLASMA_ncores=-PLASMA_ncores;
+
+      return 1;
+    }
+
+    /* calculate required memory size */
+    a_sz=m*m;
+    tot_sz=a_sz;
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+      if(buf) free(buf); /* free previously allocated memory */
+
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
+        exit(1);
+      }
+    }
+#else
+    buf_sz=tot_sz;
+    buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
+      exit(1);
+    }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+    a=buf;
+
+    /* store A into a and B into x; A is assumed to be symmetric,
+     * hence no transposition is needed
+     */
+    memcpy(a, A, a_sz*sizeof(LM_REAL));
+    memcpy(x, B, m*sizeof(LM_REAL));
+
+  /* initialize PLASMA */
+  if(PLASMA_ncores<0){
+    PLASMA_ncores=-PLASMA_ncores;
+    PLASMA_Init(PLASMA_ncores);
+    fprintf(stderr, RCAT("\n", AX_EQ_B_PLASMA_CHOL) "(): PLASMA is running on %d cores.\n\n", PLASMA_ncores);
+  }
+  
+  /* Solve the linear system */
+  info=PLASMA_POSV(PlasmaLower, m, 1, a, m, x, m);
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", PLASMA_POSV) " in ",
+                      AX_EQ_B_PLASMA_CHOL) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\n"
+                                "the factorization could not be completed for ", PLASMA_POSV) " in ", AX_EQ_B_CHOL) "()\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+      return 0;
+    }
+  }
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+#endif /* HAVE_PLASMA */
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function employs LU decomposition:
+ * If A=L U with L lower and U upper triangular, then the original system
+ * amounts to solving
+ * L y = b, U x = y
+ *
+ * A is mxm, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+int a_sz, ipiv_sz, tot_sz;
+register int i, j;
+int info, *ipiv, nrhs=1;
+LM_REAL *a;
+   
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+      return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+    /* calculate required memory size */
+    ipiv_sz=m;
+    a_sz=m*m;
+    tot_sz=a_sz*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+      if(buf) free(buf); /* free previously allocated memory */
+
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz);
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
+        exit(1);
+      }
+    }
+#else
+      buf_sz=tot_sz;
+      buf=(LM_REAL *)malloc(buf_sz);
+      if(!buf){
+        fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
+        exit(1);
+      }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+    a=buf;
+    ipiv=(int *)(a+a_sz);
+
+    /* store A (column major!) into a and B into x */
+	  for(i=0; i<m; i++){
+		  for(j=0; j<m; j++)
+        a[i+j*m]=A[i*m+j];
+
+      x[i]=B[i];
+    }
+
+  /* LU decomposition for A */
+	GETRF((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);  
+	if(info!=0){
+		if(info<0){
+      fprintf(stderr, RCAT(RCAT("argument %d of ", GETRF) " illegal in ", AX_EQ_B_LU) "()\n", -info);
+			exit(1);
+		}
+		else{
+      fprintf(stderr, RCAT(RCAT("singular matrix A for ", GETRF) " in ", AX_EQ_B_LU) "()\n");
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+			return 0;
+		}
+	}
+
+  /* solve the system with the computed LU */
+  GETRS("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
+	if(info!=0){
+		if(info<0){
+			fprintf(stderr, RCAT(RCAT("argument %d of ", GETRS) " illegal in ", AX_EQ_B_LU) "()\n", -info);
+			exit(1);
+		}
+		else{
+			fprintf(stderr, RCAT(RCAT("unknown error for ", GETRS) " in ", AX_EQ_B_LU) "()\n");
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+			return 0;
+		}
+	}
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function is based on SVD decomposition:
+ * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes
+ * (U D V^T) x = b or x=V D^{-1} U^T b
+ * Note that V D^{-1} U^T is the pseudoinverse A^+
+ *
+ * A is mxm, b is mx1.
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_SVD(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0;
+static LM_REAL eps=LM_CNST(-1.0);
+
+register int i, j;
+LM_REAL *a, *u, *s, *vt, *work;
+int a_sz, u_sz, s_sz, vt_sz, tot_sz;
+LM_REAL thresh, one_over_denom;
+register LM_REAL sum;
+int info, rank, worksz, *iwork, iworksz;
+   
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+      return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+  /* calculate required memory size */
+#if 1 /* use optimal size */
+  worksz=-1; // workspace query. Keep in mind that GESDD requires more memory than GESVD
+  /* note that optimal work size is returned in thresh */
+  GESVD("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, &info);
+  //GESDD("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, NULL, &info);
+  worksz=(int)thresh;
+#else /* use minimum size */
+  worksz=5*m; // min worksize for GESVD
+  //worksz=m*(7*m+4); // min worksize for GESDD
+#endif
+  iworksz=8*m;
+  a_sz=m*m;
+  u_sz=m*m; s_sz=m; vt_sz=m*m;
+
+  tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+    if(buf) free(buf); /* free previously allocated memory */
+
+    buf_sz=tot_sz;
+    buf=(LM_REAL *)malloc(buf_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
+      exit(1);
+    }
+  }
+#else
+    buf_sz=tot_sz;
+    buf=(LM_REAL *)malloc(buf_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
+      exit(1);
+    }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+  a=buf;
+  u=a+a_sz;
+  s=u+u_sz;
+  vt=s+s_sz;
+  work=vt+vt_sz;
+  iwork=(int *)(work+worksz);
+
+  /* store A (column major!) into a */
+  for(i=0; i<m; i++)
+    for(j=0; j<m; j++)
+      a[i+j*m]=A[i*m+j];
+
+  /* SVD decomposition of A */
+  GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
+  //GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
+
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", AX_EQ_B_SVD) "()\n", -info);
+      exit(1);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", AX_EQ_B_SVD) "() [info=%d]\n", info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+      return 0;
+    }
+  }
+
+  if(eps<0.0){
+    LM_REAL aux;
+
+    /* compute machine epsilon */
+    for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
+                                          ;
+    eps*=LM_CNST(2.0);
+  }
+
+  /* compute the pseudoinverse in a */
+	for(i=0; i<a_sz; i++) a[i]=0.0; /* initialize to zero */
+  for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
+    one_over_denom=LM_CNST(1.0)/s[rank];
+
+    for(j=0; j<m; j++)
+      for(i=0; i<m; i++)
+        a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
+  }
+
+	/* compute A^+ b in x */
+	for(i=0; i<m; i++){
+	  for(j=0, sum=0.0; j<m; j++)
+      sum+=a[i*m+j]*B[j];
+    x[i]=sum;
+  }
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+/*
+ * This function returns the solution of Ax = b for a real symmetric matrix A
+ *
+ * The function is based on LDLT factorization with the pivoting
+ * strategy of Bunch and Kaufman:
+ * A is factored as L*D*L^T where L is lower triangular and
+ * D symmetric and block diagonal (aka spectral decomposition,
+ * Banachiewicz factorization, modified Cholesky factorization)
+ *
+ * A is mxm, b is mx1.
+ *
+ * The function returns 0 in case of error, 1 if successfull
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_BK(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ LM_REAL *buf=NULL;
+__STATIC__ int buf_sz=0, nb=0;
+
+LM_REAL *a, *work;
+int a_sz, ipiv_sz, work_sz, tot_sz;
+int info, *ipiv, nrhs=1;
+   
+  if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  {
+    if(buf) free(buf);
+    buf=NULL;
+    buf_sz=0;
+
+    return 1;
+  }
+#else
+  return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+  /* calculate required memory size */
+  ipiv_sz=m;
+  a_sz=m*m;
+  if(!nb){
+    LM_REAL tmp;
+
+    work_sz=-1; // workspace query; optimal size is returned in tmp
+    SYTRF("L", (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&work_sz, (int *)&info);
+    nb=((int)tmp)/m; // optimal worksize is m*nb
+  }
+  work_sz=(nb!=-1)? nb*m : 1;
+  tot_sz=(a_sz + work_sz)*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+    if(buf) free(buf); /* free previously allocated memory */
+
+    buf_sz=tot_sz;
+    buf=(LM_REAL *)malloc(buf_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
+      exit(1);
+    }
+  }
+#else
+  buf_sz=tot_sz;
+  buf=(LM_REAL *)malloc(buf_sz);
+  if(!buf){
+    fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
+    exit(1);
+  }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+  a=buf;
+  work=a+a_sz;
+  ipiv=(int *)(work+work_sz);
+
+  /* store A into a and B into x; A is assumed to be symmetric, hence
+   * the column and row major order representations are the same
+   */
+  memcpy(a, A, a_sz*sizeof(LM_REAL));
+  memcpy(x, B, m*sizeof(LM_REAL));
+
+  /* LDLt factorization for A */
+	SYTRF("L", (int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
+	if(info!=0){
+		if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRF) " in ", AX_EQ_B_BK) "()\n", -info);
+			exit(1);
+		}
+		else{
+      fprintf(stderr, RCAT(RCAT("LAPACK error: singular block diagonal matrix D for", SYTRF) " in ", AX_EQ_B_BK)"() [D(%d, %d) is zero]\n", info, info);
+#ifndef LINSOLVERS_RETAIN_MEMORY
+      free(buf);
+#endif
+
+			return 0;
+		}
+	}
+
+  /* solve the system with the computed factorization */
+  SYTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
+  if(info<0){
+    fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRS) " in ", AX_EQ_B_BK) "()\n", -info);
+    exit(1);
+	}
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+	return 1;
+}
+
+/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
+#undef AX_EQ_B_QR
+#undef AX_EQ_B_QRLS
+#undef AX_EQ_B_CHOL
+#undef AX_EQ_B_LU
+#undef AX_EQ_B_SVD
+#undef AX_EQ_B_BK
+#undef AX_EQ_B_PLASMA_CHOL
+
+#undef GEQRF
+#undef ORGQR
+#undef TRTRS
+#undef POTF2
+#undef POTRF
+#undef POTRS
+#undef GETRF
+#undef GETRS
+#undef GESVD
+#undef GESDD
+#undef SYTRF
+#undef SYTRS
+#undef PLASMA_POSV
+
+#else // no LAPACK
+
+/* precision-specific definitions */
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
+
+/*
+ * This function returns the solution of Ax = b
+ *
+ * The function employs LU decomposition followed by forward/back substitution (see 
+ * also the LAPACK-based LU solver above)
+ *
+ * A is mxm, b is mx1
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ * This function is often called repetitively to solve problems of identical
+ * dimensions. To avoid repetitive malloc's and free's, allocated memory is
+ * retained between calls and free'd-malloc'ed when not of the appropriate size.
+ * A call with NULL as the first argument forces this memory to be released.
+ */
+int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
+{
+__STATIC__ void *buf=NULL;
+__STATIC__ int buf_sz=0;
+
+register int i, j, k;
+int *idx, maxi=-1, idx_sz, a_sz, work_sz, tot_sz;
+LM_REAL *a, *work, max, sum, tmp;
+
+    if(!A)
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    {
+      if(buf) free(buf);
+      buf=NULL;
+      buf_sz=0;
+
+      return 1;
+    }
+#else
+    return 1; /* NOP */
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+   
+  /* calculate required memory size */
+  idx_sz=m;
+  a_sz=m*m;
+  work_sz=m;
+  tot_sz=(a_sz+work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
+    if(buf) free(buf); /* free previously allocated memory */
+
+    buf_sz=tot_sz;
+    buf=(void *)malloc(tot_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
+      exit(1);
+    }
+  }
+#else
+    buf_sz=tot_sz;
+    buf=(void *)malloc(tot_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
+      exit(1);
+    }
+#endif /* LINSOLVERS_RETAIN_MEMORY */
+
+  a=buf;
+  work=a+a_sz;
+  idx=(int *)(work+work_sz);
+
+  /* avoid destroying A, B by copying them to a, x resp. */
+  memcpy(a, A, a_sz*sizeof(LM_REAL));
+  memcpy(x, B, m*sizeof(LM_REAL));
+
+  /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
+	for(i=0; i<m; ++i){
+		max=0.0;
+		for(j=0; j<m; ++j)
+			if((tmp=FABS(a[i*m+j]))>max)
+        max=tmp;
+		  if(max==0.0){
+        fprintf(stderr, RCAT("Singular matrix A in ", AX_EQ_B_LU) "()!\n");
+#ifndef LINSOLVERS_RETAIN_MEMORY
+        free(buf);
+#endif
+
+        return 0;
+      }
+		  work[i]=LM_CNST(1.0)/max;
+	}
+
+	for(j=0; j<m; ++j){
+		for(i=0; i<j; ++i){
+			sum=a[i*m+j];
+			for(k=0; k<i; ++k)
+        sum-=a[i*m+k]*a[k*m+j];
+			a[i*m+j]=sum;
+		}
+		max=0.0;
+		for(i=j; i<m; ++i){
+			sum=a[i*m+j];
+			for(k=0; k<j; ++k)
+        sum-=a[i*m+k]*a[k*m+j];
+			a[i*m+j]=sum;
+			if((tmp=work[i]*FABS(sum))>=max){
+				max=tmp;
+				maxi=i;
+			}
+		}
+		if(j!=maxi){
+			for(k=0; k<m; ++k){
+				tmp=a[maxi*m+k];
+				a[maxi*m+k]=a[j*m+k];
+				a[j*m+k]=tmp;
+			}
+			work[maxi]=work[j];
+		}
+		idx[j]=maxi;
+		if(a[j*m+j]==0.0)
+      a[j*m+j]=LM_REAL_EPSILON;
+		if(j!=m-1){
+			tmp=LM_CNST(1.0)/(a[j*m+j]);
+			for(i=j+1; i<m; ++i)
+        a[i*m+j]*=tmp;
+		}
+	}
+
+  /* The decomposition has now replaced a. Solve the linear system using
+   * forward and back substitution
+   */
+	for(i=k=0; i<m; ++i){
+		j=idx[i];
+		sum=x[j];
+		x[j]=x[i];
+		if(k!=0)
+			for(j=k-1; j<i; ++j)
+        sum-=a[i*m+j]*x[j];
+		else
+      if(sum!=0.0)
+			  k=i+1;
+		x[i]=sum;
+	}
+
+	for(i=m-1; i>=0; --i){
+		sum=x[i];
+		for(j=i+1; j<m; ++j)
+      sum-=a[i*m+j]*x[j];
+		x[i]=sum/a[i*m+i];
+	}
+
+#ifndef LINSOLVERS_RETAIN_MEMORY
+  free(buf);
+#endif
+
+  return 1;
+}
+
+/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
+#undef AX_EQ_B_LU
+
+#endif /* HAVE_LAPACK */
Index: ThirdParty/levmar/src/Makefile.am
===================================================================
--- ThirdParty/levmar/src/Makefile.am	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/Makefile.am	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,86 @@
+
+AM_LDFLAGS = $(LDFLAGS)
+AM_CFLAGS = $(CFLAGS)
+
+LIBSOURCES = \
+	Axb.c \
+	lm.c \
+	lmbc.c \
+	lmblec.c \
+	lmbleic.c \
+	lmlec.c \
+	misc.c
+
+LIBCORESOURCES = \
+	Axb_core.c \
+	lm_core.c \
+	lmbc_core.c \
+	lmblec_core.c \
+	lmbleic_core.c \
+	lmlec_core.c \
+	misc_core.c
+
+EXTRA_DIST = \
+	$(LIBCORESOURCES)
+
+LIBHEADERS = \
+	compiler.h \
+	levmar.h \
+	misc.h
+
+lib_LTLIBRARIES = liblevmar.la
+liblevmar_la_includedir = $(includedir)/levmar/
+liblevmar_la_CFLAGS = \
+	$(AM_CFLAGS)
+liblevmar_la_LDFLAGS = \
+	$(AM_LDFLAGS)
+liblevmar_la_LIBADD = \
+	$(LAPACK_LIBS) \
+	$(BLAS_LIBS) \
+	$(F2C_LIBS) \
+	$(LIBS)
+nobase_liblevmar_la_include_HEADERS = $(LIBHEADERS)
+
+## Define the source file list for the "libexample-@MOLECUILDER_API_VERSION@.la"
+## target.  Note that @MOLECUILDER_API_VERSION@ is not interpreted by Automake and
+## will therefore be treated as if it were literally part of the target name,
+## and the variable name derived from that.
+## The file extension .cc is recognized by Automake, and makes it produce
+## rules which invoke the C++ compiler to produce a libtool object file (.lo)
+## from each source file.  Note that it is not necessary to list header files
+## which are already listed elsewhere in a _HEADERS variable assignment.
+liblevmar_la_SOURCES = ${LIBSOURCES}
+liblevmar_la_DEPENDENCIES = $(LIBCORESOURCES)
+
+## Instruct libtool to include ABI version information in the generated shared
+## library file (.so).  The library ABI version is defined in configure.ac, so
+## that all version information is kept in one place.
+liblevmar_la_LDFLAGS += -version-info $(LEVMAR_SO_VERSION)
+
+## The generated configuration header is installed in its own subdirectory of
+## $(libdir).  The reason for this is that the configuration information put
+## into this header file describes the target platform the installed library
+## has been built for.  Thus the file must not be installed into a location
+## intended for architecture-independent files, as defined by the Filesystem
+## Hierarchy Standard (FHS).
+## The nodist_ prefix instructs Automake to not generate rules for including
+## the listed files in the distribution on 'make dist'.  Files that are listed
+## in _HEADERS variables are normally included in the distribution, but the
+## configuration header file is generated at configure time and should not be
+## shipped with the source tarball.
+liblevmar_la_libincludedir = $(libdir)/levmar/include
+nodist_liblevmar_la_libinclude_HEADERS = $(top_builddir)/liblevmar_config.h
+
+## Install the generated pkg-config file (.pc) into the expected location for
+## architecture-dependent package configuration information.  Occasionally,
+## pkg-config files are also used for architecture-independent data packages,
+## in which case the correct install location would be $(datadir)/pkgconfig.
+pkgconfigdir = $(libdir)/pkgconfig
+pkgconfig_DATA = $(top_builddir)/levmar.pc
+
+
+bin_PROGRAMS = lmdemo
+
+lmdemo_SOURCES = lmdemo.c levmar.h
+lmdemo_LDFLAGS = $(AM_LDFLAGS)
+lmdemo_LDADD = liblevmar.la
Index: ThirdParty/levmar/src/compiler.h
===================================================================
--- ThirdParty/levmar/src/compiler.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/compiler.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,49 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _COMPILER_H_
+#define _COMPILER_H_
+
+/* note: intel's icc defines both __ICC & __INTEL_COMPILER.
+ * Also, some compilers other than gcc define __GNUC__,
+ * therefore gcc should be checked last
+ */
+#ifdef _MSC_VER
+#define inline __inline // MSVC
+#elif !defined(__ICC) && !defined(__INTEL_COMPILER) && !defined(__GNUC__)
+#define inline // other than MSVC, ICC, GCC: define empty
+#endif
+
+#ifdef _MSC_VER
+#define LM_FINITE _finite // MSVC
+#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)
+#define LM_FINITE finite // ICC, GCC
+#else
+#define LM_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work
+#endif
+
+#ifdef _MSC_VER
+#define LM_ISINF(x) (!_finite(x) && !_isnan(x)) // MSVC
+#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)
+#define LM_ISINF(x) isinf(x) // ICC, GCC
+#else
+#define LM_ISINF(x) isinf(x) // other than MSVC, ICC, GCC, let's hope this will work
+#endif
+
+#endif /* _COMPILER_H_ */
Index: ThirdParty/levmar/src/expfit.c
===================================================================
--- ThirdParty/levmar/src/expfit.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/expfit.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,122 @@
+////////////////////////////////////////////////////////////////////////////////////
+//  Example program that shows how to use levmar in order to fit the three-
+//  parameter exponential model x_i = p[0]*exp(-p[1]*i) + p[2] to a set of
+//  data measurements; example is based on a similar one from GSL.
+//
+//  Copyright (C) 2008  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+////////////////////////////////////////////////////////////////////////////////////
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <levmar.h>
+
+#ifndef LM_DBL_PREC
+#error Example program assumes that levmar has been compiled with double precision, see LM_DBL_PREC!
+#endif
+
+
+/* the following macros concern the initialization of a random number generator for adding noise */
+#undef REPEATABLE_RANDOM
+#define DBL_RAND_MAX (double)(RAND_MAX)
+
+#ifdef _MSC_VER // MSVC
+#include <process.h>
+#define GETPID  _getpid
+#elif defined(__GNUC__) // GCC
+#include <sys/types.h>
+#include <unistd.h>
+#define GETPID  getpid
+#else
+#warning Do not know the name of the function returning the process id for your OS/compiler combination
+#define GETPID  0
+#endif /* _MSC_VER */
+
+#ifdef REPEATABLE_RANDOM
+#define INIT_RANDOM(seed) srandom(seed)
+#else
+#define INIT_RANDOM(seed) srandom((int)GETPID()) // seed unused
+#endif
+
+/* Gaussian noise with mean m and variance s, uses the Box-Muller transformation */
+double gNoise(double m, double s)
+{
+double r1, r2, val;
+
+  r1=((double)random())/DBL_RAND_MAX;
+  r2=((double)random())/DBL_RAND_MAX;
+
+  val=sqrt(-2.0*log(r1))*cos(2.0*M_PI*r2);
+
+  val=s*val+m;
+
+  return val;
+}
+
+/* model to be fitted to measurements: x_i = p[0]*exp(-p[1]*i) + p[2], i=0...n-1 */
+void expfunc(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; ++i){
+    x[i]=p[0]*exp(-p[1]*i) + p[2];
+  }
+}
+
+/* Jacobian of expfunc() */
+void jacexpfunc(double *p, double *jac, int m, int n, void *data)
+{   
+register int i, j;
+  
+  /* fill Jacobian row by row */
+  for(i=j=0; i<n; ++i){
+    jac[j++]=exp(-p[1]*i);
+    jac[j++]=-p[0]*i*exp(-p[1]*i);
+    jac[j++]=1.0;
+  }
+}
+
+int main()
+{
+const int n=40, m=3; // 40 measurements, 3 parameters
+double p[m], x[n], opts[LM_OPTS_SZ], info[LM_INFO_SZ];
+register int i;
+int ret;
+
+  /* generate some measurement using the exponential model with
+   * parameters (5.0, 0.1, 1.0), corrupted with zero-mean
+   * Gaussian noise of s=0.1
+   */
+  INIT_RANDOM(0);
+  for(i=0; i<n; ++i)
+    x[i]=(5.0*exp(-0.1*i) + 1.0) + gNoise(0.0, 0.1);
+
+  /* initial parameters estimate: (1.0, 0.0, 0.0) */
+  p[0]=1.0; p[1]=0.0; p[2]=0.0;
+
+  /* optimization control parameters; passing to levmar NULL instead of opts reverts to defaults */
+  opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
+  opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used 
+
+  /* invoke the optimization function */
+  ret=dlevmar_der(expfunc, jacexpfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+  //ret=dlevmar_dif(expfunc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // without Jacobian
+  printf("Levenberg-Marquardt returned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6], info[1], info[0]);
+  printf("Best fit parameters: %.7g %.7g %.7g\n", p[0], p[1], p[2]);
+
+  exit(0);
+}
Index: ThirdParty/levmar/src/levmar.h
===================================================================
--- ThirdParty/levmar/src/levmar.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/levmar.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,398 @@
+/* 
+////////////////////////////////////////////////////////////////////////////////////
+// 
+//  Prototypes and definitions for the Levenberg - Marquardt minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+////////////////////////////////////////////////////////////////////////////////////
+*/
+
+#ifndef _LEVMAR_H_
+#define _LEVMAR_H_
+
+/************************************* Start of configuration options *************************************/
+/* Note that when compiling with CMake, this configuration section is automatically generated
+ * based on the user's input, see levmar.h.in
+ */
+
+/* specifies whether to use LAPACK or not. Using LAPACK is strongly recommended */
+#define HAVE_LAPACK
+
+/* specifies whether the PLASMA parallel library for multicore CPUs is available */
+/* #undef HAVE_PLASMA */
+                      
+/* to avoid the overhead of repeated mallocs(), routines in Axb.c can be instructed to
+ * retain working memory between calls. Such a choice, however, renders these routines
+ * non-reentrant and is not safe in a shared memory multiprocessing environment.
+ * Bellow, an attempt is made to issue a warning if this option is turned on and OpenMP
+ * is being used (note that this will work only if omp.h is included before levmar.h)
+ */
+#define LINSOLVERS_RETAIN_MEMORY
+#if (defined(_OPENMP))
+# ifdef LINSOLVERS_RETAIN_MEMORY
+#  ifdef _MSC_VER
+#  pragma message("LINSOLVERS_RETAIN_MEMORY is not safe in a multithreaded environment and should be turned off!")
+#  else
+#  warning LINSOLVERS_RETAIN_MEMORY is not safe in a multithreaded environment and should be turned off!
+#  endif /* _MSC_VER */
+# endif /* LINSOLVERS_RETAIN_MEMORY */
+#endif /* _OPENMP */
+
+/* specifies whether double precision routines will be compiled or not */
+#define LM_DBL_PREC
+/* specifies whether single precision routines will be compiled or not */
+#define LM_SNGL_PREC
+
+/****************** End of configuration options, no changes necessary beyond this point ******************/
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* work arrays size for ?levmar_der and ?levmar_dif functions.
+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
+ */
+#define LM_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
+#define LM_DIF_WORKSZ(npar, nmeas) (4*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
+
+/* work arrays size for ?levmar_bc_der and ?levmar_bc_dif functions.
+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
+ */
+#define LM_BC_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
+#define LM_BC_DIF_WORKSZ(npar, nmeas) LM_BC_DER_WORKSZ((npar), (nmeas)) /* LEVMAR_BC_DIF currently implemented using LEVMAR_BC_DER()! */
+
+/* work arrays size for ?levmar_lec_der and ?levmar_lec_dif functions.
+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
+ */
+#define LM_LEC_DER_WORKSZ(npar, nmeas, nconstr) LM_DER_WORKSZ((npar)-(nconstr), (nmeas))
+#define LM_LEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_DIF_WORKSZ((npar)-(nconstr), (nmeas))
+
+/* work arrays size for ?levmar_blec_der and ?levmar_blec_dif functions.
+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
+ */
+#define LM_BLEC_DER_WORKSZ(npar, nmeas, nconstr) LM_LEC_DER_WORKSZ((npar), (nmeas)+(npar), (nconstr))
+#define LM_BLEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_LEC_DIF_WORKSZ((npar), (nmeas)+(npar), (nconstr))
+
+/* work arrays size for ?levmar_bleic_der and ?levmar_bleic_dif functions.
+ * should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
+ */
+#define LM_BLEIC_DER_WORKSZ(npar, nmeas, nconstr1, nconstr2) LM_BLEC_DER_WORKSZ((npar)+(nconstr2), (nmeas)+(nconstr2), (nconstr1)+(nconstr2))
+#define LM_BLEIC_DIF_WORKSZ(npar, nmeas, nconstr1, nconstr2) LM_BLEC_DIF_WORKSZ((npar)+(nconstr2), (nmeas)+(nconstr2), (nconstr1)+(nconstr2))
+
+#define LM_OPTS_SZ    	 5 /* max(4, 5) */
+#define LM_INFO_SZ    	 10
+#define LM_ERROR         -1
+#define LM_INIT_MU    	 1E-03
+#define LM_STOP_THRESH	 1E-17
+#define LM_DIFF_DELTA    1E-06
+#define LM_VERSION       "2.6 (November 2011)"
+
+#ifdef LM_DBL_PREC
+/* double precision LM, with & without Jacobian */
+/* unconstrained minimization */
+extern int dlevmar_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, int itmax, double *opts,
+      double *info, double *work, double *covar, void *adata);
+
+extern int dlevmar_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, int itmax, double *opts,
+      double *info, double *work, double *covar, void *adata);
+
+/* box-constrained minimization */
+extern int dlevmar_bc_der(
+       void (*func)(double *p, double *hx, int m, int n, void *adata),
+       void (*jacf)(double *p, double *j, int m, int n, void *adata),  
+       double *p, double *x, int m, int n, double *lb, double *ub, double *dscl,
+       int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+extern int dlevmar_bc_dif(
+       void (*func)(double *p, double *hx, int m, int n, void *adata),
+       double *p, double *x, int m, int n, double *lb, double *ub, double *dscl,
+       int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+#ifdef HAVE_LAPACK
+/* linear equation constrained minimization */
+extern int dlevmar_lec_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *A, double *b, int k,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+extern int dlevmar_lec_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *A, double *b, int k,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+/* box & linear equation constrained minimization */
+extern int dlevmar_blec_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+extern int dlevmar_blec_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+/* box, linear equations & inequalities constrained minimization */
+extern int dlevmar_bleic_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub,
+      double *A, double *b, int k1, double *C, double *d, int k2,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+extern int dlevmar_bleic_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub, 
+      double *A, double *b, int k1, double *C, double *d, int k2,
+      int itmax, double *opts, double *info, double *work, double *covar, void *adata);
+
+/* box & linear inequality constraints */
+extern int dlevmar_blic_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub, double *C, double *d, int k2,
+      int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+
+extern int dlevmar_blic_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *lb, double *ub, double *C, double *d, int k2,
+      int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+
+/* linear equation & inequality constraints */
+extern int dlevmar_leic_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *A, double *b, int k1, double *C, double *d, int k2,
+      int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+
+extern int dlevmar_leic_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *A, double *b, int k1, double *C, double *d, int k2,
+      int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+
+/* linear inequality constraints */
+extern int dlevmar_lic_der(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      void (*jacf)(double *p, double *j, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *C, double *d, int k2,
+      int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+
+extern int dlevmar_lic_dif(
+      void (*func)(double *p, double *hx, int m, int n, void *adata),
+      double *p, double *x, int m, int n, double *C, double *d, int k2,
+      int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
+#endif /* HAVE_LAPACK */
+
+#endif /* LM_DBL_PREC */
+
+
+#ifdef LM_SNGL_PREC
+/* single precision LM, with & without Jacobian */
+/* unconstrained minimization */
+extern int slevmar_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, int itmax, float *opts,
+      float *info, float *work, float *covar, void *adata);
+
+extern int slevmar_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, int itmax, float *opts,
+      float *info, float *work, float *covar, void *adata);
+
+/* box-constrained minimization */
+extern int slevmar_bc_der(
+       void (*func)(float *p, float *hx, int m, int n, void *adata),
+       void (*jacf)(float *p, float *j, int m, int n, void *adata),  
+       float *p, float *x, int m, int n, float *lb, float *ub, float *dscl,
+       int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+extern int slevmar_bc_dif(
+       void (*func)(float *p, float *hx, int m, int n, void *adata),
+       float *p, float *x, int m, int n, float *lb, float *ub, float *dscl,
+       int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+#ifdef HAVE_LAPACK
+/* linear equation constrained minimization */
+extern int slevmar_lec_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *A, float *b, int k,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+extern int slevmar_lec_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *A, float *b, int k,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+/* box & linear equation constrained minimization */
+extern int slevmar_blec_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+extern int slevmar_blec_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+/* box, linear equations & inequalities constrained minimization */
+extern int slevmar_bleic_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub,
+      float *A, float *b, int k1, float *C, float *d, int k2,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+extern int slevmar_bleic_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub,
+      float *A, float *b, int k1, float *C, float *d, int k2,
+      int itmax, float *opts, float *info, float *work, float *covar, void *adata);
+
+/* box & linear inequality constraints */
+extern int slevmar_blic_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub, float *C, float *d, int k2,
+      int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+
+extern int slevmar_blic_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *lb, float *ub, float *C, float *d, int k2,
+      int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+
+/* linear equality & inequality constraints */
+extern int slevmar_leic_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *A, float *b, int k1, float *C, float *d, int k2,
+      int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+
+extern int slevmar_leic_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *A, float *b, int k1, float *C, float *d, int k2,
+      int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+
+/* linear inequality constraints */
+extern int slevmar_lic_der(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      void (*jacf)(float *p, float *j, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *C, float *d, int k2,
+      int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+
+extern int slevmar_lic_dif(
+      void (*func)(float *p, float *hx, int m, int n, void *adata),
+      float *p, float *x, int m, int n, float *C, float *d, int k2,
+      int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
+#endif /* HAVE_LAPACK */
+
+#endif /* LM_SNGL_PREC */
+
+/* linear system solvers */
+#ifdef HAVE_LAPACK
+
+#ifdef LM_DBL_PREC
+extern int dAx_eq_b_QR(double *A, double *B, double *x, int m);
+extern int dAx_eq_b_QRLS(double *A, double *B, double *x, int m, int n);
+extern int dAx_eq_b_Chol(double *A, double *B, double *x, int m);
+extern int dAx_eq_b_LU(double *A, double *B, double *x, int m);
+extern int dAx_eq_b_SVD(double *A, double *B, double *x, int m);
+extern int dAx_eq_b_BK(double *A, double *B, double *x, int m);
+#endif /* LM_DBL_PREC */
+
+#ifdef LM_SNGL_PREC
+extern int sAx_eq_b_QR(float *A, float *B, float *x, int m);
+extern int sAx_eq_b_QRLS(float *A, float *B, float *x, int m, int n);
+extern int sAx_eq_b_Chol(float *A, float *B, float *x, int m);
+extern int sAx_eq_b_LU(float *A, float *B, float *x, int m);
+extern int sAx_eq_b_SVD(float *A, float *B, float *x, int m);
+extern int sAx_eq_b_BK(float *A, float *B, float *x, int m);
+#endif /* LM_SNGL_PREC */
+
+#else /* no LAPACK */
+
+#ifdef LM_DBL_PREC
+extern int dAx_eq_b_LU_noLapack(double *A, double *B, double *x, int n);
+#endif /* LM_DBL_PREC */
+
+#ifdef LM_SNGL_PREC
+extern int sAx_eq_b_LU_noLapack(float *A, float *B, float *x, int n);
+#endif /* LM_SNGL_PREC */
+
+#endif /* HAVE_LAPACK */
+
+#ifdef HAVE_PLASMA
+#ifdef LM_DBL_PREC
+extern int dAx_eq_b_PLASMA_Chol(double *A, double *B, double *x, int m);
+#endif
+#ifdef LM_SNGL_PREC
+extern int sAx_eq_b_PLASMA_Chol(float *A, float *B, float *x, int m);
+#endif
+extern void levmar_PLASMA_setnbcores(int cores);
+#endif /* HAVE_PLASMA */
+
+/* Jacobian verification, double & single precision */
+#ifdef LM_DBL_PREC
+extern void dlevmar_chkjac(
+    void (*func)(double *p, double *hx, int m, int n, void *adata),
+    void (*jacf)(double *p, double *j, int m, int n, void *adata),
+    double *p, int m, int n, void *adata, double *err);
+#endif /* LM_DBL_PREC */
+
+#ifdef LM_SNGL_PREC
+extern void slevmar_chkjac(
+    void (*func)(float *p, float *hx, int m, int n, void *adata),
+    void (*jacf)(float *p, float *j, int m, int n, void *adata),
+    float *p, int m, int n, void *adata, float *err);
+#endif /* LM_SNGL_PREC */
+
+/* miscellaneous: standard deviation, coefficient of determination (R2),
+ *                Pearson's correlation coefficient for best-fit parameters
+ */
+#ifdef LM_DBL_PREC
+extern double dlevmar_stddev( double *covar, int m, int i);
+extern double dlevmar_corcoef(double *covar, int m, int i, int j);
+extern double dlevmar_R2(void (*func)(double *p, double *hx, int m, int n, void *adata), double *p, double *x, int m, int n, void *adata);
+
+#endif /* LM_DBL_PREC */
+
+#ifdef LM_SNGL_PREC
+extern float slevmar_stddev( float *covar, int m, int i);
+extern float slevmar_corcoef(float *covar, int m, int i, int j);
+extern float slevmar_R2(void (*func)(float *p, float *hx, int m, int n, void *adata), float *p, float *x, int m, int n, void *adata);
+
+extern void slevmar_locscale(
+        void (*func)(float *p, float *hx, int m, int n, void *adata),
+        float *p, float *x, int m, int n, void *adata,
+        int howto, float locscl[2], float **residptr);
+
+extern int slevmar_outlid(float *r, int n, float thresh, float ls[2], char *outlmap);
+
+#endif /* LM_SNGL_PREC */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _LEVMAR_H_ */
Index: ThirdParty/levmar/src/lm.c
===================================================================
--- ThirdParty/levmar/src/lm.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lm.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,83 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * Levenberg-Marquardt nonlinear minimization. The same core code is used with
+ * appropriate #defines to derive single and double precision versions, see
+ * also lm_core.c
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+#include "compiler.h"
+#include "misc.h"
+
+#define EPSILON       1E-12
+#define ONE_THIRD     0.3333333334 /* 1.0/3.0 */
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define LM_REAL_MAX FLT_MAX
+#define LM_REAL_MIN -FLT_MAX
+#define LM_REAL_EPSILON FLT_EPSILON
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "lm_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_EPSILON
+#undef LM_REAL_MIN
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_REAL_MAX DBL_MAX
+#define LM_REAL_MIN -DBL_MAX
+#define LM_REAL_EPSILON DBL_EPSILON
+#define LM_CNST(x) (x)
+
+#include "lm_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_EPSILON
+#undef LM_REAL_MIN
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
Index: ThirdParty/levmar/src/lm.h
===================================================================
--- ThirdParty/levmar/src/lm.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lm.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,11 @@
+#ifndef _DEPR_LM_H_
+#define _DEPR_LM_H_
+
+#ifdef _MSC_VER
+#pragma message("lm.h is deprecated, please use levmar.h instead!")
+#else
+#error lm.h is deprecated, please use levmar.h instead!
+#endif /* _MSC_VER */
+
+#endif /* _DEPR_LM_H_ */
+
Index: ThirdParty/levmar/src/lm_core.c
===================================================================
--- ThirdParty/levmar/src/lm_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lm_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,858 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LM_REAL // not included by lm.c
+#error This file should not be compiled directly!
+#endif
+
+
+/* precision-specific definitions */
+#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
+#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)
+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+
+#ifdef HAVE_LAPACK
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
+#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
+#else
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
+#endif /* HAVE_LAPACK */
+
+#ifdef HAVE_PLASMA
+#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)
+#endif
+
+/* 
+ * This function seeks the parameter vector p that best describes the measurements vector x.
+ * More precisely, given a vector function  func : R^m --> R^n with n>=m,
+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
+ * e=x-func(p) is minimized.
+ *
+ * This function requires an analytic Jacobian. In case the latter is unavailable,
+ * use LEVMAR_DIF() bellow
+ *
+ * Returns the number of iterations (>=0) if successful, LM_ERROR if failed
+ *
+ * For more details, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on 
+ * non-linear least squares at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
+ */
+
+int LEVMAR_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
+                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_DER_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
+                      * Set to NULL if not needed
+                      */
+{
+register int i, j, k, l;
+int worksz, freework=0, issolved;
+/* temp work arrays */
+LM_REAL *e,          /* nx1 */
+       *hx,         /* \hat{x}_i, nx1 */
+       *jacTe,      /* J^T e_i mx1 */
+       *jac,        /* nxm */
+       *jacTjac,    /* mxm */
+       *Dp,         /* mx1 */
+   *diag_jacTjac,   /* diagonal of J^T J, mx1 */
+       *pDp;        /* p + Dp, mx1 */
+
+register LM_REAL mu,  /* damping constant */
+                tmp; /* mainly used in matrix & vector multiplications */
+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
+LM_REAL tau, eps1, eps2, eps2_sq, eps3;
+LM_REAL init_p_eL2;
+int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
+const int nm=n*m;
+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
+
+  mu=jacTe_inf=0.0; /* -Wall */
+
+  if(n<m){
+    fprintf(stderr, LCAT(LEVMAR_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
+    return LM_ERROR;
+  }
+
+  if(!jacf){
+    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_DER)
+        RCAT("().\nIf no such function is available, use ", LEVMAR_DIF) RCAT("() rather than ", LEVMAR_DER) "()\n");
+    return LM_ERROR;
+  }
+
+  if(opts){
+	  tau=opts[0];
+	  eps1=opts[1];
+	  eps2=opts[2];
+	  eps2_sq=opts[2]*opts[2];
+    eps3=opts[3];
+  }
+  else{ // use default values
+	  tau=LM_CNST(LM_INIT_MU);
+	  eps1=LM_CNST(LM_STOP_THRESH);
+	  eps2=LM_CNST(LM_STOP_THRESH);
+	  eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
+    eps3=LM_CNST(LM_STOP_THRESH);
+  }
+
+  if(!work){
+    worksz=LM_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
+    work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
+    if(!work){
+      fprintf(stderr, LCAT(LEVMAR_DER, "(): memory allocation request failed\n"));
+      return LM_ERROR;
+    }
+    freework=1;
+  }
+
+  /* set up work arrays */
+  e=work;
+  hx=e + n;
+  jacTe=hx + n;
+  jac=jacTe + m;
+  jacTjac=jac + nm;
+  Dp=jacTjac + m*m;
+  diag_jacTjac=Dp + m;
+  pDp=diag_jacTjac + m;
+
+  /* compute e=x - f(p) and its L2 norm */
+  (*func)(p, hx, m, n, adata); nfev=1;
+  /* ### e=x-hx, p_eL2=||e|| */
+#if 1
+  p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);  
+#else
+  for(i=0, p_eL2=0.0; i<n; ++i){
+    e[i]=tmp=x[i]-hx[i];
+    p_eL2+=tmp*tmp;
+  }
+#endif
+  init_p_eL2=p_eL2;
+  if(!LM_FINITE(p_eL2)) stop=7;
+
+  for(k=0; k<itmax && !stop; ++k){
+    /* Note that p and e have been updated at a previous iteration */
+
+    if(p_eL2<=eps3){ /* error is small */
+      stop=6;
+      break;
+    }
+
+    /* Compute the Jacobian J at p,  J^T J,  J^T e,  ||J^T e||_inf and ||p||^2.
+     * Since J^T J is symmetric, its computation can be sped up by computing
+     * only its upper triangular part and copying it to the lower part
+     */
+
+    (*jacf)(p, jac, m, n, adata); ++njev;
+
+    /* J^T J, J^T e */
+    if(nm<__BLOCKSZ__SQ){ // this is a small problem
+      /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.
+       * Thus, the product J^T J can be computed using an outer loop for
+       * l that adds J_li*J_lj to each element ij of the result. Note that
+       * with this scheme, the accesses to J and JtJ are always along rows,
+       * therefore induces less cache misses compared to the straightforward
+       * algorithm for computing the product (i.e., l loop is innermost one).
+       * A similar scheme applies to the computation of J^T e.
+       * However, for large minimization problems (i.e., involving a large number
+       * of unknowns and measurements) for which J/J^T J rows are too large to
+       * fit in the L1 cache, even this scheme incures many cache misses. In
+       * such cases, a cache-efficient blocking scheme is preferable.
+       *
+       * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
+       * performance problem.
+       *
+       * Note that the non-blocking algorithm is faster on small
+       * problems since in this case it avoids the overheads of blocking. 
+       */
+
+      /* looping downwards saves a few computations */
+      register int l;
+      register LM_REAL alpha, *jaclm, *jacTjacim;
+
+      for(i=m*m; i-->0; )
+        jacTjac[i]=0.0;
+      for(i=m; i-->0; )
+        jacTe[i]=0.0;
+
+      for(l=n; l-->0; ){
+        jaclm=jac+l*m;
+        for(i=m; i-->0; ){
+          jacTjacim=jacTjac+i*m;
+          alpha=jaclm[i]; //jac[l*m+i];
+          for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
+            jacTjacim[j]+=jaclm[j]*alpha; //jacTjac[i*m+j]+=jac[l*m+j]*alpha
+
+          /* J^T e */
+          jacTe[i]+=alpha*e[l];
+        }
+      }
+
+      for(i=m; i-->0; ) /* copy to upper part */
+        for(j=i+1; j<m; ++j)
+          jacTjac[i*m+j]=jacTjac[j*m+i];
+
+    }
+    else{ // this is a large problem
+      /* Cache efficient computation of J^T J based on blocking
+       */
+      LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
+
+      /* cache efficient computation of J^T e */
+      for(i=0; i<m; ++i)
+        jacTe[i]=0.0;
+
+      for(i=0; i<n; ++i){
+        register LM_REAL *jacrow;
+
+        for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
+          jacTe[l]+=jacrow[l]*tmp;
+      }
+    }
+
+	  /* Compute ||J^T e||_inf and ||p||^2 */
+    for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
+      if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
+
+      diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
+      p_L2+=p[i]*p[i];
+    }
+    //p_L2=sqrt(p_L2);
+
+#if 0
+if(!(k%100)){
+  printf("Current estimate: ");
+  for(i=0; i<m; ++i)
+    printf("%.9g ", p[i]);
+  printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
+}
+#endif
+
+    /* check for convergence */
+    if((jacTe_inf <= eps1)){
+      Dp_L2=0.0; /* no increment for p in this case */
+      stop=1;
+      break;
+    }
+
+   /* compute initial damping factor */
+    if(k==0){
+      for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+        if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
+      mu=tau*tmp;
+    }
+
+    /* determine increment using adaptive damping */
+    while(1){
+      /* augment normal equations */
+      for(i=0; i<m; ++i)
+        jacTjac[i*m+i]+=mu;
+
+      /* solve augmented equations */
+#ifdef HAVE_LAPACK
+      /* 7 alternatives are available: LU, Cholesky + Cholesky with PLASMA, LDLt, 2 variants of QR decomposition and SVD.
+       * For matrices with dimensions of at least a few hundreds, the PLASMA implementation of Cholesky is the fastest.
+       * From the serial solvers, Cholesky is the fastest but might occasionally be inapplicable due to numerical round-off;
+       * QR is slower but more robust; SVD is the slowest but most robust; LU is quite robust but
+       * slower than LDLt; LDLt offers a good tradeoff between robustness and speed
+       */
+
+      issolved=AX_EQ_B_BK(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_BK;
+      //issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+      //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;
+#ifdef HAVE_PLASMA
+      //issolved=AX_EQ_B_PLASMA_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_PLASMA_CHOL;
+#endif
+      //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;
+      //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;
+      //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;
+
+#else
+      /* use the LU included with levmar */
+      issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+#endif /* HAVE_LAPACK */
+
+      if(issolved){
+        /* compute p's new estimate and ||Dp||^2 */
+        for(i=0, Dp_L2=0.0; i<m; ++i){
+          pDp[i]=p[i] + (tmp=Dp[i]);
+          Dp_L2+=tmp*tmp;
+        }
+        //Dp_L2=sqrt(Dp_L2);
+
+        if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
+        //if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
+          stop=2;
+          break;
+        }
+
+       if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
+       //if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
+         stop=4;
+         break;
+       }
+
+        (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
+        /* compute ||e(pDp)||_2 */
+        /* ### hx=x-hx, pDp_eL2=||hx|| */
+#if 1
+        pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
+#else
+        for(i=0, pDp_eL2=0.0; i<n; ++i){
+          hx[i]=tmp=x[i]-hx[i];
+          pDp_eL2+=tmp*tmp;
+        }
+#endif
+        if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
+                                  * This check makes sure that the inner loop does not run indefinitely.
+                                  * Thanks to Steve Danauskas for reporting such cases
+                                  */
+          stop=7;
+          break;
+        }
+
+        for(i=0, dL=0.0; i<m; ++i)
+          dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
+
+        dF=p_eL2-pDp_eL2;
+
+        if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
+          tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
+          tmp=LM_CNST(1.0)-tmp*tmp*tmp;
+          mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
+          nu=2;
+
+          for(i=0 ; i<m; ++i) /* update p's estimate */
+            p[i]=pDp[i];
+
+          for(i=0; i<n; ++i) /* update e and ||e||_2 */
+            e[i]=hx[i];
+          p_eL2=pDp_eL2;
+          break;
+        }
+      }
+
+      /* if this point is reached, either the linear system could not be solved or
+       * the error did not reduce; in any case, the increment must be rejected
+       */
+
+      mu*=nu;
+      nu2=nu<<1; // 2*nu;
+      if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
+        stop=5;
+        break;
+      }
+      nu=nu2;
+
+      for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+        jacTjac[i*m+i]=diag_jacTjac[i];
+    } /* inner loop */
+  }
+
+  if(k>=itmax) stop=3;
+
+  for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+    jacTjac[i*m+i]=diag_jacTjac[i];
+
+  if(info){
+    info[0]=init_p_eL2;
+    info[1]=p_eL2;
+    info[2]=jacTe_inf;
+    info[3]=Dp_L2;
+    for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+      if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
+    info[4]=mu/tmp;
+    info[5]=(LM_REAL)k;
+    info[6]=(LM_REAL)stop;
+    info[7]=(LM_REAL)nfev;
+    info[8]=(LM_REAL)njev;
+    info[9]=(LM_REAL)nlss;
+  }
+
+  /* covariance matrix */
+  if(covar){
+    LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
+  }
+
+  if(freework) free(work);
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
+#endif
+
+  return (stop!=4 && stop!=7)?  k : LM_ERROR;
+}
+
+
+/* Secant version of the LEVMAR_DER() function above: the Jacobian is approximated with 
+ * the aid of finite differences (forward or central, see the comment for the opts argument)
+ */
+int LEVMAR_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[5],    /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
+                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
+                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
+                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
+                       * (but slower!) compared to the forward differences employed by default. 
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_DIF_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
+                      * Set to NULL if not needed
+                      */
+{
+register int i, j, k, l;
+int worksz, freework=0, issolved;
+/* temp work arrays */
+LM_REAL *e,          /* nx1 */
+       *hx,         /* \hat{x}_i, nx1 */
+       *jacTe,      /* J^T e_i mx1 */
+       *jac,        /* nxm */
+       *jacTjac,    /* mxm */
+       *Dp,         /* mx1 */
+   *diag_jacTjac,   /* diagonal of J^T J, mx1 */
+       *pDp,        /* p + Dp, mx1 */
+       *wrk,        /* nx1 */
+       *wrk2;       /* nx1, used only for holding a temporary e vector and when differentiating with central differences */
+
+int using_ffdif=1;
+
+register LM_REAL mu,  /* damping constant */
+                tmp; /* mainly used in matrix & vector multiplications */
+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
+LM_REAL tau, eps1, eps2, eps2_sq, eps3, delta;
+LM_REAL init_p_eL2;
+int nu, nu2, stop=0, nfev, njap=0, nlss=0, K=(m>=10)? m: 10, updjac, updp=1, newjac;
+const int nm=n*m;
+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
+
+  mu=jacTe_inf=p_L2=0.0; /* -Wall */
+  updjac=newjac=0; /* -Wall */
+
+  if(n<m){
+    fprintf(stderr, LCAT(LEVMAR_DIF, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
+    return LM_ERROR;
+  }
+
+  if(opts){
+	  tau=opts[0];
+	  eps1=opts[1];
+	  eps2=opts[2];
+	  eps2_sq=opts[2]*opts[2];
+    eps3=opts[3];
+	  delta=opts[4];
+    if(delta<0.0){
+      delta=-delta; /* make positive */
+      using_ffdif=0; /* use central differencing */
+    }
+  }
+  else{ // use default values
+	  tau=LM_CNST(LM_INIT_MU);
+	  eps1=LM_CNST(LM_STOP_THRESH);
+	  eps2=LM_CNST(LM_STOP_THRESH);
+	  eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
+    eps3=LM_CNST(LM_STOP_THRESH);
+	  delta=LM_CNST(LM_DIFF_DELTA);
+  }
+
+  if(!work){
+    worksz=LM_DIF_WORKSZ(m, n); //4*n+4*m + n*m + m*m;
+    work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
+    if(!work){
+      fprintf(stderr, LCAT(LEVMAR_DIF, "(): memory allocation request failed\n"));
+      return LM_ERROR;
+    }
+    freework=1;
+  }
+
+  /* set up work arrays */
+  e=work;
+  hx=e + n;
+  jacTe=hx + n;
+  jac=jacTe + m;
+  jacTjac=jac + nm;
+  Dp=jacTjac + m*m;
+  diag_jacTjac=Dp + m;
+  pDp=diag_jacTjac + m;
+  wrk=pDp + m;
+  wrk2=wrk + n;
+
+  /* compute e=x - f(p) and its L2 norm */
+  (*func)(p, hx, m, n, adata); nfev=1;
+  /* ### e=x-hx, p_eL2=||e|| */
+#if 1
+  p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
+#else
+  for(i=0, p_eL2=0.0; i<n; ++i){
+    e[i]=tmp=x[i]-hx[i];
+    p_eL2+=tmp*tmp;
+  }
+#endif
+  init_p_eL2=p_eL2;
+  if(!LM_FINITE(p_eL2)) stop=7;
+
+  nu=20; /* force computation of J */
+
+  for(k=0; k<itmax && !stop; ++k){
+    /* Note that p and e have been updated at a previous iteration */
+
+    if(p_eL2<=eps3){ /* error is small */
+      stop=6;
+      break;
+    }
+
+    /* Compute the Jacobian J at p,  J^T J,  J^T e,  ||J^T e||_inf and ||p||^2.
+     * The symmetry of J^T J is again exploited for speed
+     */
+
+    if((updp && nu>16) || updjac==K){ /* compute difference approximation to J */
+      if(using_ffdif){ /* use forward differences */
+        LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, delta, jac, m, n, adata);
+        ++njap; nfev+=m;
+      }
+      else{ /* use central differences */
+        LEVMAR_FDIF_CENT_JAC_APPROX(func, p, wrk, wrk2, delta, jac, m, n, adata);
+        ++njap; nfev+=2*m;
+      }
+      nu=2; updjac=0; updp=0; newjac=1;
+    }
+
+    if(newjac){ /* Jacobian has changed, recompute J^T J, J^t e, etc */
+      newjac=0;
+
+      /* J^T J, J^T e */
+      if(nm<=__BLOCKSZ__SQ){ // this is a small problem
+        /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.
+         * Thus, the product J^T J can be computed using an outer loop for
+         * l that adds J_li*J_lj to each element ij of the result. Note that
+         * with this scheme, the accesses to J and JtJ are always along rows,
+         * therefore induces less cache misses compared to the straightforward
+         * algorithm for computing the product (i.e., l loop is innermost one).
+         * A similar scheme applies to the computation of J^T e.
+         * However, for large minimization problems (i.e., involving a large number
+         * of unknowns and measurements) for which J/J^T J rows are too large to
+         * fit in the L1 cache, even this scheme incures many cache misses. In
+         * such cases, a cache-efficient blocking scheme is preferable.
+         *
+         * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
+         * performance problem.
+         *
+         * Note that the non-blocking algorithm is faster on small
+         * problems since in this case it avoids the overheads of blocking. 
+         */
+        register int l;
+        register LM_REAL alpha, *jaclm, *jacTjacim;
+
+        /* looping downwards saves a few computations */
+        for(i=m*m; i-->0; )
+          jacTjac[i]=0.0;
+        for(i=m; i-->0; )
+          jacTe[i]=0.0;
+
+        for(l=n; l-->0; ){
+          jaclm=jac+l*m;
+          for(i=m; i-->0; ){
+            jacTjacim=jacTjac+i*m;
+            alpha=jaclm[i]; //jac[l*m+i];
+            for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
+              jacTjacim[j]+=jaclm[j]*alpha; //jacTjac[i*m+j]+=jac[l*m+j]*alpha
+
+            /* J^T e */
+            jacTe[i]+=alpha*e[l];
+          }
+        }
+
+        for(i=m; i-->0; ) /* copy to upper part */
+          for(j=i+1; j<m; ++j)
+            jacTjac[i*m+j]=jacTjac[j*m+i];
+      }
+      else{ // this is a large problem
+        /* Cache efficient computation of J^T J based on blocking
+         */
+        LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
+
+        /* cache efficient computation of J^T e */
+        for(i=0; i<m; ++i)
+          jacTe[i]=0.0;
+
+        for(i=0; i<n; ++i){
+          register LM_REAL *jacrow;
+
+          for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
+            jacTe[l]+=jacrow[l]*tmp;
+        }
+      }
+      
+      /* Compute ||J^T e||_inf and ||p||^2 */
+      for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
+        if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
+
+        diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
+        p_L2+=p[i]*p[i];
+      }
+      //p_L2=sqrt(p_L2);
+    }
+
+#if 0
+if(!(k%100)){
+  printf("Current estimate: ");
+  for(i=0; i<m; ++i)
+    printf("%.9g ", p[i]);
+  printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
+}
+#endif
+
+    /* check for convergence */
+    if((jacTe_inf <= eps1)){
+      Dp_L2=0.0; /* no increment for p in this case */
+      stop=1;
+      break;
+    }
+
+   /* compute initial damping factor */
+    if(k==0){
+      for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+        if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
+      mu=tau*tmp;
+    }
+
+    /* determine increment using adaptive damping */
+
+    /* augment normal equations */
+    for(i=0; i<m; ++i)
+      jacTjac[i*m+i]+=mu;
+
+    /* solve augmented equations */
+#ifdef HAVE_LAPACK
+    /* 7 alternatives are available: LU, Cholesky + Cholesky with PLASMA, LDLt, 2 variants of QR decomposition and SVD.
+     * For matrices with dimensions of at least a few hundreds, the PLASMA implementation of Cholesky is the fastest.
+     * From the serial solvers, Cholesky is the fastest but might occasionally be inapplicable due to numerical round-off;
+     * QR is slower but more robust; SVD is the slowest but most robust; LU is quite robust but
+     * slower than LDLt; LDLt offers a good tradeoff between robustness and speed
+     */
+
+    issolved=AX_EQ_B_BK(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_BK;
+    //issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+    //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;
+#ifdef HAVE_PLASMA
+    //issolved=AX_EQ_B_PLASMA_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_PLASMA_CHOL;
+#endif
+    //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;
+    //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;
+    //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;
+#else
+    /* use the LU included with levmar */
+    issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+#endif /* HAVE_LAPACK */
+
+    if(issolved){
+    /* compute p's new estimate and ||Dp||^2 */
+      for(i=0, Dp_L2=0.0; i<m; ++i){
+        pDp[i]=p[i] + (tmp=Dp[i]);
+        Dp_L2+=tmp*tmp;
+      }
+      //Dp_L2=sqrt(Dp_L2);
+
+      if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
+      //if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
+        stop=2;
+        break;
+      }
+
+      if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
+      //if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
+        stop=4;
+        break;
+      }
+
+      (*func)(pDp, wrk, m, n, adata); ++nfev; /* evaluate function at p + Dp */
+      /* compute ||e(pDp)||_2 */
+      /* ### wrk2=x-wrk, pDp_eL2=||wrk2|| */
+#if 1
+      pDp_eL2=LEVMAR_L2NRMXMY(wrk2, x, wrk, n);
+#else
+      for(i=0, pDp_eL2=0.0; i<n; ++i){
+        wrk2[i]=tmp=x[i]-wrk[i];
+        pDp_eL2+=tmp*tmp;
+      }
+#endif
+      if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
+                                * This check makes sure that the loop terminates early in the case
+                                * of invalid input. Thanks to Steve Danauskas for suggesting it
+                                */
+
+        stop=7;
+        break;
+      }
+
+      dF=p_eL2-pDp_eL2;
+      if(updp || dF>0){ /* update jac */
+        for(i=0; i<n; ++i){
+          for(l=0, tmp=0.0; l<m; ++l)
+            tmp+=jac[i*m+l]*Dp[l]; /* (J * Dp)[i] */
+          tmp=(wrk[i] - hx[i] - tmp)/Dp_L2; /* (f(p+dp)[i] - f(p)[i] - (J * Dp)[i])/(dp^T*dp) */
+          for(j=0; j<m; ++j)
+            jac[i*m+j]+=tmp*Dp[j];
+        }
+        ++updjac;
+        newjac=1;
+      }
+
+      for(i=0, dL=0.0; i<m; ++i)
+        dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
+
+      if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
+        tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
+        tmp=LM_CNST(1.0)-tmp*tmp*tmp;
+        mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
+        nu=2;
+
+        for(i=0 ; i<m; ++i) /* update p's estimate */
+          p[i]=pDp[i];
+
+        for(i=0; i<n; ++i){ /* update e, hx and ||e||_2 */
+          e[i]=wrk2[i]; //x[i]-wrk[i];
+          hx[i]=wrk[i];
+        }
+        p_eL2=pDp_eL2;
+        updp=1;
+        continue;
+      }
+    }
+
+    /* if this point is reached, either the linear system could not be solved or
+     * the error did not reduce; in any case, the increment must be rejected
+     */
+
+    mu*=nu;
+    nu2=nu<<1; // 2*nu;
+    if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
+      stop=5;
+      break;
+    }
+    nu=nu2;
+
+    for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+      jacTjac[i*m+i]=diag_jacTjac[i];
+  }
+
+  if(k>=itmax) stop=3;
+
+  for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+    jacTjac[i*m+i]=diag_jacTjac[i];
+
+  if(info){
+    info[0]=init_p_eL2;
+    info[1]=p_eL2;
+    info[2]=jacTe_inf;
+    info[3]=Dp_L2;
+    for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+      if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
+    info[4]=mu/tmp;
+    info[5]=(LM_REAL)k;
+    info[6]=(LM_REAL)stop;
+    info[7]=(LM_REAL)nfev;
+    info[8]=(LM_REAL)njap;
+    info[9]=(LM_REAL)nlss;
+  }
+
+  /* covariance matrix */
+  if(covar){
+    LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
+  }
+
+                                                               
+  if(freework) free(work);
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+  if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
+#endif
+
+  return (stop!=4 && stop!=7)?  k : LM_ERROR;
+}
+
+/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef LEVMAR_DER
+#undef LEVMAR_DIF
+#undef LEVMAR_FDIF_FORW_JAC_APPROX
+#undef LEVMAR_FDIF_CENT_JAC_APPROX
+#undef LEVMAR_COVAR
+#undef LEVMAR_TRANS_MAT_MAT_MULT
+#undef LEVMAR_L2NRMXMY
+#undef AX_EQ_B_LU
+#undef AX_EQ_B_CHOL
+#undef AX_EQ_B_PLASMA_CHOL
+#undef AX_EQ_B_QR
+#undef AX_EQ_B_QRLS
+#undef AX_EQ_B_SVD
+#undef AX_EQ_B_BK
Index: ThirdParty/levmar/src/lmbc.c
===================================================================
--- ThirdParty/levmar/src/lmbc.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmbc.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,87 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * Box-constrained Levenberg-Marquardt nonlinear minimization. The same core code
+ * is used with appropriate #defines to derive single and double precision versions,
+ * see also lmbc_core.c
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+#include "compiler.h"
+#include "misc.h"
+
+#define EPSILON       1E-12
+#define ONE_THIRD     0.3333333334 /* 1.0/3.0 */
+#define _LSITMAX_     150 /* max #iterations for line search */
+#define _POW_         2.1
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define LM_REAL_MAX FLT_MAX
+#define LM_REAL_MIN -FLT_MAX
+
+#define LM_REAL_EPSILON FLT_EPSILON
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "lmbc_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef LM_REAL_EPSILON
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_REAL_MAX DBL_MAX
+#define LM_REAL_MIN -DBL_MAX
+
+#define LM_REAL_EPSILON DBL_EPSILON
+#define LM_CNST(x) (x)
+
+#include "lmbc_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef LM_REAL_EPSILON
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
Index: ThirdParty/levmar/src/lmbc_core.c
===================================================================
--- ThirdParty/levmar/src/lmbc_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmbc_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,1154 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LM_REAL // not included by lmbc.c
+#error This file should not be compiled directly!
+#endif
+
+
+/* precision-specific definitions */
+#define FUNC_STATE LM_ADD_PREFIX(func_state)
+#define LNSRCH LM_ADD_PREFIX(lnsrch)
+#define BOXPROJECT LM_ADD_PREFIX(boxProject)
+#define BOXSCALE LM_ADD_PREFIX(boxScale)
+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
+#define VECNORM LM_ADD_PREFIX(vecnorm)
+#define LEVMAR_BC_DER LM_ADD_PREFIX(levmar_bc_der)
+#define LEVMAR_BC_DIF LM_ADD_PREFIX(levmar_bc_dif)
+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+#define LMBC_DIF_DATA LM_ADD_PREFIX(lmbc_dif_data)
+#define LMBC_DIF_FUNC LM_ADD_PREFIX(lmbc_dif_func)
+#define LMBC_DIF_JACF LM_ADD_PREFIX(lmbc_dif_jacf)
+
+#ifdef HAVE_LAPACK
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
+#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
+#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
+#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
+#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
+#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
+#else
+#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
+#endif /* HAVE_LAPACK */
+
+#ifdef HAVE_PLASMA
+#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)
+#endif
+
+/* find the median of 3 numbers */
+#define __MEDIAN3(a, b, c) ( ((a) >= (b))?\
+        ( ((c) >= (a))? (a) : ( ((c) <= (b))? (b) : (c) ) ) : \
+        ( ((c) >= (b))? (b) : ( ((c) <= (a))? (a) : (c) ) ) )
+
+/* Projections to feasible set \Omega: P_{\Omega}(y) := arg min { ||x - y|| : x \in \Omega},  y \in R^m */
+
+/* project vector p to a box shaped feasible set. p is a mx1 vector.
+ * Either lb, ub can be NULL. If not NULL, they are mx1 vectors
+ */
+static void BOXPROJECT(LM_REAL *p, LM_REAL *lb, LM_REAL *ub, int m)
+{
+register int i;
+
+  if(!lb){ /* no lower bounds */
+    if(!ub) /* no upper bounds */
+      return;
+    else{ /* upper bounds only */
+      for(i=m; i-->0; )
+        if(p[i]>ub[i]) p[i]=ub[i];
+    }
+  }
+  else
+    if(!ub){ /* lower bounds only */
+      for(i=m; i-->0; )
+        if(p[i]<lb[i]) p[i]=lb[i];
+    }
+    else /* box bounds */
+      for(i=m; i-->0; )
+        p[i]=__MEDIAN3(lb[i], p[i], ub[i]);
+}
+#undef __MEDIAN3
+
+/* pointwise scaling of bounds with the mx1 vector scl. If div=1 scaling is by 1./scl.
+ * Either lb, ub can be NULL. If not NULL, they are mx1 vectors
+ */
+static void BOXSCALE(LM_REAL *lb, LM_REAL *ub, LM_REAL *scl, int m, int div)
+{
+register int i;
+
+  if(!lb){ /* no lower bounds */
+    if(!ub) /* no upper bounds */
+      return;
+    else{ /* upper bounds only */
+      if(div){
+        for(i=m; i-->0; )
+          if(ub[i]!=LM_REAL_MAX)
+            ub[i]=ub[i]/scl[i];
+      }else{
+        for(i=m; i-->0; )
+          if(ub[i]!=LM_REAL_MAX)
+            ub[i]=ub[i]*scl[i];
+      }
+    }
+  }
+  else
+    if(!ub){ /* lower bounds only */
+      if(div){
+        for(i=m; i-->0; )
+          if(lb[i]!=LM_REAL_MIN)
+            lb[i]=lb[i]/scl[i];
+      }else{
+        for(i=m; i-->0; )
+          if(lb[i]!=LM_REAL_MIN)
+            lb[i]=lb[i]*scl[i];
+      }
+    }
+    else{ /* box bounds */
+      if(div){
+        for(i=m; i-->0; ){
+          if(ub[i]!=LM_REAL_MAX)
+            ub[i]=ub[i]/scl[i];
+          if(lb[i]!=LM_REAL_MIN)
+            lb[i]=lb[i]/scl[i];
+        }
+      }else{
+        for(i=m; i-->0; ){
+          if(ub[i]!=LM_REAL_MAX)
+            ub[i]=ub[i]*scl[i];
+          if(lb[i]!=LM_REAL_MIN)
+            lb[i]=lb[i]*scl[i];
+        }
+      }
+    }
+}
+
+/* compute the norm of a vector in a manner that avoids overflows
+ */
+static LM_REAL VECNORM(LM_REAL *x, int n)
+{
+#ifdef HAVE_LAPACK
+#define NRM2 LM_MK_BLAS_NAME(nrm2)
+extern LM_REAL NRM2(int *n, LM_REAL *dx, int *incx);
+int one=1;
+
+  return NRM2(&n, x, &one);
+#undef NRM2
+#else // no LAPACK, use the simple method described by Blue in TOMS78
+register int i;
+LM_REAL max, sum, tmp;
+
+  for(i=n, max=0.0; i-->0; )
+    if(x[i]>max) max=x[i];
+    else if(x[i]<-max) max=-x[i];
+
+  for(i=n, sum=0.0; i-->0; ){
+    tmp=x[i]/max;
+    sum+=tmp*tmp;
+  }
+
+  return max*(LM_REAL)sqrt(sum);
+#endif /* HAVE_LAPACK */
+}
+
+struct FUNC_STATE{
+  int n, *nfev;
+  LM_REAL *hx, *x;
+  LM_REAL *lb, *ub;
+  void *adata;
+};
+
+static void
+LNSRCH(int m, LM_REAL *x, LM_REAL f, LM_REAL *g, LM_REAL *p, LM_REAL alpha, LM_REAL *xpls,
+       LM_REAL *ffpls, void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), struct FUNC_STATE *state,
+       int *mxtake, int *iretcd, LM_REAL stepmx, LM_REAL steptl, LM_REAL *sx)
+{
+/* Find a next newton iterate by backtracking line search.
+ * Specifically, finds a \lambda such that for a fixed alpha<0.5 (usually 1e-4),
+ * f(x + \lambda*p) <= f(x) + alpha * \lambda * g^T*p
+ *
+ * Translated (with a few changes) from Schnabel, Koontz & Weiss uncmin.f,  v1.3
+ * Main changes include the addition of box projection and modification of the scaling 
+ * logic since uncmin.f operates in the original (unscaled) variable space.
+
+ * PARAMETERS :
+
+ *	m       --> dimension of problem (i.e. number of variables)
+ *	x(m)    --> old iterate:	x[k-1]
+ *	f       --> function value at old iterate, f(x)
+ *	g(m)    --> gradient at old iterate, g(x), or approximate
+ *	p(m)    --> non-zero newton step
+ *	alpha   --> fixed constant < 0.5 for line search (see above)
+ *	xpls(m) <--	 new iterate x[k]
+ *	ffpls   <--	 function value at new iterate, f(xpls)
+ *	func    --> name of subroutine to evaluate function
+ *	state   <--> information other than x and m that func requires.
+ *			    state is not modified in xlnsrch (but can be modified by func).
+ *	iretcd  <--	 return code
+ *	mxtake  <--	 boolean flag indicating step of maximum length used
+ *	stepmx  --> maximum allowable step size
+ *	steptl  --> relative step size at which successive iterates
+ *			    considered close enough to terminate algorithm
+ *	sx(m)	  --> diagonal scaling matrix for x, can be NULL
+
+ *	internal variables
+
+ *	sln		 newton length
+ *	rln		 relative length of newton step
+*/
+
+    register int i, j;
+    int firstback = 1;
+    LM_REAL disc;
+    LM_REAL a3, b;
+    LM_REAL t1, t2, t3, lambda, tlmbda, rmnlmb;
+    LM_REAL scl, rln, sln, slp;
+    LM_REAL tmp1, tmp2;
+    LM_REAL fpls, pfpls = 0., plmbda = 0.; /* -Wall */
+
+    f*=LM_CNST(0.5);
+    *mxtake = 0;
+    *iretcd = 2;
+    tmp1 = 0.;
+    for (i = m; i-- > 0;  )
+      tmp1 += p[i] * p[i];
+    sln = (LM_REAL)sqrt(tmp1);
+    if (sln > stepmx) {
+	  /*	newton step longer than maximum allowed */
+	    scl = stepmx / sln;
+      for (i = m; i-- > 0;  ) /* p * scl */
+        p[i]*=scl;
+	    sln = stepmx;
+    }
+    for (i = m, slp = rln = 0.; i-- > 0;  ){
+      slp+=g[i]*p[i]; /* g^T * p */
+
+      tmp1 = (FABS(x[i])>=LM_CNST(1.))? FABS(x[i]) : LM_CNST(1.);
+      tmp2 = FABS(p[i])/tmp1;
+      if(rln < tmp2) rln = tmp2;
+    }
+    rmnlmb = steptl / rln;
+    lambda = LM_CNST(1.0);
+
+    /*	check if new iterate satisfactory.  generate new lambda if necessary. */
+
+    for(j = _LSITMAX_; j-- > 0;  ) {
+      for (i = m; i-- > 0;  )
+        xpls[i] = x[i] + lambda * p[i];
+      BOXPROJECT(xpls, state->lb, state->ub, m); /* project to feasible set */
+
+      /* evaluate function at new point */
+      if(!sx){
+        (*func)(xpls, state->hx, m, state->n, state->adata); ++(*(state->nfev));
+      }
+      else{
+        for (i = m; i-- > 0;  ) xpls[i] *= sx[i];
+        (*func)(xpls, state->hx, m, state->n, state->adata); ++(*(state->nfev));
+        for (i = m; i-- > 0;  ) xpls[i] /= sx[i];
+      }
+      /* ### state->hx=state->x-state->hx, tmp1=||state->hx|| */
+#if 1
+       tmp1=LEVMAR_L2NRMXMY(state->hx, state->x, state->hx, state->n);
+#else
+      for(i=0, tmp1=0.0; i<state->n; ++i){
+        state->hx[i]=tmp2=state->x[i]-state->hx[i];
+        tmp1+=tmp2*tmp2;
+      }
+#endif
+      fpls=LM_CNST(0.5)*tmp1; *ffpls=tmp1;
+
+	    if (fpls <= f + slp * alpha * lambda) { /* solution found */
+	      *iretcd = 0;
+	      if (lambda == LM_CNST(1.) && sln > stepmx * LM_CNST(.99)) *mxtake = 1;
+	      return;
+	    }
+
+	    /* else : solution not (yet) found */
+
+      /* First find a point with a finite value */
+
+	    if (lambda < rmnlmb) {
+	      /* no satisfactory xpls found sufficiently distinct from x */
+
+	      *iretcd = 1;
+	      return;
+	    }
+	    else { /*	calculate new lambda */
+
+	      /* modifications to cover non-finite values */
+	      if (!LM_FINITE(fpls)) {
+		      lambda *= LM_CNST(0.1);
+		      firstback = 1;
+	      }
+	      else {
+		      if (firstback) { /*	first backtrack: quadratic fit */
+		        tlmbda = -lambda * slp / ((fpls - f - slp) * LM_CNST(2.));
+		        firstback = 0;
+		      }
+		      else { /*	all subsequent backtracks: cubic fit */
+		        t1 = fpls - f - lambda * slp;
+		        t2 = pfpls - f - plmbda * slp;
+		        t3 = LM_CNST(1.) / (lambda - plmbda);
+		        a3 = LM_CNST(3.) * t3 * (t1 / (lambda * lambda)
+				      - t2 / (plmbda * plmbda));
+		        b = t3 * (t2 * lambda / (plmbda * plmbda)
+			          - t1 * plmbda / (lambda * lambda));
+		        disc = b * b - a3 * slp;
+		        if (disc > b * b)
+			      /* only one positive critical point, must be minimum */
+			        tlmbda = (-b + ((a3 < 0)? -(LM_REAL)sqrt(disc): (LM_REAL)sqrt(disc))) /a3;
+		        else
+			      /* both critical points positive, first is minimum */
+			        tlmbda = (-b + ((a3 < 0)? (LM_REAL)sqrt(disc): -(LM_REAL)sqrt(disc))) /a3;
+
+		        if (tlmbda > lambda * LM_CNST(.5))
+			        tlmbda = lambda * LM_CNST(.5);
+		      }
+		      plmbda = lambda;
+		      pfpls = fpls;
+		      if (tlmbda < lambda * LM_CNST(.1))
+		        lambda *= LM_CNST(.1);
+		      else
+		        lambda = tlmbda;
+        }
+	    }
+    }
+    /* this point is reached when the iterations limit is exceeded */
+	  *iretcd = 1; /* failed */
+	  return;
+} /* LNSRCH */
+
+/* 
+ * This function seeks the parameter vector p that best describes the measurements
+ * vector x under box constraints.
+ * More precisely, given a vector function  func : R^m --> R^n with n>=m,
+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
+ * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i].
+ * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
+ * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
+ *
+ * This function requires an analytic Jacobian. In case the latter is unavailable,
+ * use LEVMAR_BC_DIF() bellow
+ *
+ * Returns the number of iterations (>=0) if successful, LM_ERROR if failed
+ *
+ * For details, see C. Kanzow, N. Yamashita and M. Fukushima: "Levenberg-Marquardt
+ * methods for constrained nonlinear equations with strong local convergence properties",
+ * Journal of Computational and Applied Mathematics 172, 2004, pp. 375-397.
+ * Also, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on 
+ * unconstrained Levenberg-Marquardt at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
+ *
+ * The algorithm implemented by this function employs projected gradient steps. Since steepest descent
+ * is very sensitive to poor scaling, diagonal scaling has been implemented through the dscl argument:
+ * Instead of minimizing f(p) for p, f(D*q) is minimized for q=D^-1*p, D being a diagonal scaling
+ * matrix whose diagonal equals dscl (see Nocedal-Wright p.27). dscl should contain "typical" magnitudes 
+ * for the parameters p. A NULL value for dscl implies no scaling. i.e. D=I.
+ * To account for scaling, the code divides the starting point and box bounds pointwise by dscl. Moreover,
+ * before calling func and jacf the scaling has to be undone (by multiplying), as should be done with
+ * the final point. Note also that jac_q=jac_p*D, where jac_q, jac_p are the jacobians w.r.t. q & p, resp.
+ */
+
+int LEVMAR_BC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *dscl,      /* I: diagonal scaling constants. NULL implies no scaling */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
+                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used.
+                       * Note that ||J^T e||_inf is computed on free (not equal to lb[i] or ub[i]) variables only.
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BC_DER_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
+                      * Set to NULL if not needed
+                      */
+{
+register int i, j, k, l;
+int worksz, freework=0, issolved;
+/* temp work arrays */
+LM_REAL *e,          /* nx1 */
+       *hx,         /* \hat{x}_i, nx1 */
+       *jacTe,      /* J^T e_i mx1 */
+       *jac,        /* nxm */
+       *jacTjac,    /* mxm */
+       *Dp,         /* mx1 */
+   *diag_jacTjac,   /* diagonal of J^T J, mx1 */
+       *pDp,        /* p + Dp, mx1 */
+   *sp_pDp=NULL;    /* dscl*p or dscl*pDp, mx1 */
+
+register LM_REAL mu,  /* damping constant */
+                tmp; /* mainly used in matrix & vector multiplications */
+LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
+LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
+LM_REAL tau, eps1, eps2, eps2_sq, eps3;
+LM_REAL init_p_eL2;
+int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
+const int nm=n*m;
+
+/* variables for constrained LM */
+struct FUNC_STATE fstate;
+LM_REAL alpha=LM_CNST(1e-4), beta=LM_CNST(0.9), gamma=LM_CNST(0.99995), rho=LM_CNST(1e-8);
+LM_REAL t, t0, jacTeDp;
+LM_REAL tmin=LM_CNST(1e-12), tming=LM_CNST(1e-18); /* minimum step length for LS and PG steps */
+const LM_REAL tini=LM_CNST(1.0); /* initial step length for LS and PG steps */
+int nLMsteps=0, nLSsteps=0, nPGsteps=0, gprevtaken=0;
+int numactive;
+int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
+
+  mu=jacTe_inf=t=0.0;  tmin=tmin; /* -Wall */
+
+  if(n<m){
+    fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
+    return LM_ERROR;
+  }
+
+  if(!jacf){
+    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BC_DER)
+        RCAT("().\nIf no such function is available, use ", LEVMAR_BC_DIF) RCAT("() rather than ", LEVMAR_BC_DER) "()\n");
+    return LM_ERROR;
+  }
+
+  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
+    fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): at least one lower bound exceeds the upper one\n"));
+    return LM_ERROR;
+  }
+
+  if(dscl){ /* check that scaling consts are valid */
+    for(i=m; i-->0; )
+      if(dscl[i]<=0.0){
+        fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): scaling constants should be positive (scale %d: %g <= 0)\n"), i, dscl[i]);
+        return LM_ERROR;
+      }
+
+    sp_pDp=(LM_REAL *)malloc(m*sizeof(LM_REAL));
+    if(!sp_pDp){
+      fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): memory allocation request failed\n"));
+      return LM_ERROR;
+    }
+  }
+
+  if(opts){
+	  tau=opts[0];
+	  eps1=opts[1];
+	  eps2=opts[2];
+	  eps2_sq=opts[2]*opts[2];
+	  eps3=opts[3];
+  }
+  else{ // use default values
+	  tau=LM_CNST(LM_INIT_MU);
+	  eps1=LM_CNST(LM_STOP_THRESH);
+	  eps2=LM_CNST(LM_STOP_THRESH);
+	  eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
+	  eps3=LM_CNST(LM_STOP_THRESH);
+  }
+
+  if(!work){
+    worksz=LM_BC_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
+    work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
+    if(!work){
+      fprintf(stderr, LCAT(LEVMAR_BC_DER, "(): memory allocation request failed\n"));
+      return LM_ERROR;
+    }
+    freework=1;
+  }
+
+  /* set up work arrays */
+  e=work;
+  hx=e + n;
+  jacTe=hx + n;
+  jac=jacTe + m;
+  jacTjac=jac + nm;
+  Dp=jacTjac + m*m;
+  diag_jacTjac=Dp + m;
+  pDp=diag_jacTjac + m;
+
+  fstate.n=n;
+  fstate.hx=hx;
+  fstate.x=x;
+  fstate.lb=lb;
+  fstate.ub=ub;
+  fstate.adata=adata;
+  fstate.nfev=&nfev;
+  
+  /* see if starting point is within the feasible set */
+  for(i=0; i<m; ++i)
+    pDp[i]=p[i];
+  BOXPROJECT(p, lb, ub, m); /* project to feasible set */
+  for(i=0; i<m; ++i)
+    if(pDp[i]!=p[i])
+      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_BC_DER) "()! [%g projected to %g]\n",
+                      i, pDp[i], p[i]);
+
+  /* compute e=x - f(p) and its L2 norm */
+  (*func)(p, hx, m, n, adata); nfev=1;
+  /* ### e=x-hx, p_eL2=||e|| */
+#if 1
+  p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
+#else
+  for(i=0, p_eL2=0.0; i<n; ++i){
+    e[i]=tmp=x[i]-hx[i];
+    p_eL2+=tmp*tmp;
+  }
+#endif
+  init_p_eL2=p_eL2;
+  if(!LM_FINITE(p_eL2)) stop=7;
+
+  if(dscl){
+    /* scale starting point and constraints */
+    for(i=m; i-->0; ) p[i]/=dscl[i];
+    BOXSCALE(lb, ub, dscl, m, 1);
+  }
+
+  for(k=0; k<itmax && !stop; ++k){
+    /* Note that p and e have been updated at a previous iteration */
+
+    if(p_eL2<=eps3){ /* error is small */
+      stop=6;
+      break;
+    }
+
+    /* Compute the Jacobian J at p,  J^T J,  J^T e,  ||J^T e||_inf and ||p||^2.
+     * Since J^T J is symmetric, its computation can be sped up by computing
+     * only its upper triangular part and copying it to the lower part
+     */
+
+    if(!dscl){
+      (*jacf)(p, jac, m, n, adata); ++njev;
+    }
+    else{
+      for(i=m; i-->0; ) sp_pDp[i]=p[i]*dscl[i];
+      (*jacf)(sp_pDp, jac, m, n, adata); ++njev;
+
+      /* compute jac*D */
+      for(i=n; i-->0; ){
+        register LM_REAL *jacim;
+
+        jacim=jac+i*m;
+        for(j=m; j-->0; )
+          jacim[j]*=dscl[j]; // jac[i*m+j]*=dscl[j];
+      }
+    }
+
+    /* J^T J, J^T e */
+    if(nm<__BLOCKSZ__SQ){ // this is a small problem
+      /* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.
+       * Thus, the product J^T J can be computed using an outer loop for
+       * l that adds J_li*J_lj to each element ij of the result. Note that
+       * with this scheme, the accesses to J and JtJ are always along rows,
+       * therefore induces less cache misses compared to the straightforward
+       * algorithm for computing the product (i.e., l loop is innermost one).
+       * A similar scheme applies to the computation of J^T e.
+       * However, for large minimization problems (i.e., involving a large number
+       * of unknowns and measurements) for which J/J^T J rows are too large to
+       * fit in the L1 cache, even this scheme incures many cache misses. In
+       * such cases, a cache-efficient blocking scheme is preferable.
+       *
+       * Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
+       * performance problem.
+       *
+       * Note that the non-blocking algorithm is faster on small
+       * problems since in this case it avoids the overheads of blocking. 
+       */
+      register LM_REAL alpha, *jaclm, *jacTjacim;
+
+      /* looping downwards saves a few computations */
+      for(i=m*m; i-->0; )
+        jacTjac[i]=0.0;
+      for(i=m; i-->0; )
+        jacTe[i]=0.0;
+
+      for(l=n; l-->0; ){
+        jaclm=jac+l*m;
+        for(i=m; i-->0; ){
+          jacTjacim=jacTjac+i*m;
+          alpha=jaclm[i]; //jac[l*m+i];
+          for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
+            jacTjacim[j]+=jaclm[j]*alpha; //jacTjac[i*m+j]+=jac[l*m+j]*alpha
+
+          /* J^T e */
+          jacTe[i]+=alpha*e[l];
+        }
+      }
+
+      for(i=m; i-->0; ) /* copy to upper part */
+        for(j=i+1; j<m; ++j)
+          jacTjac[i*m+j]=jacTjac[j*m+i];
+    }
+    else{ // this is a large problem
+      /* Cache efficient computation of J^T J based on blocking
+       */
+      LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
+
+      /* cache efficient computation of J^T e */
+      for(i=0; i<m; ++i)
+        jacTe[i]=0.0;
+
+      for(i=0; i<n; ++i){
+        register LM_REAL *jacrow;
+
+        for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
+          jacTe[l]+=jacrow[l]*tmp;
+      }
+    }
+
+	  /* Compute ||J^T e||_inf and ||p||^2. Note that ||J^T e||_inf
+     * is computed for free (i.e. inactive) variables only. 
+     * At a local minimum, if p[i]==ub[i] then g[i]>0;
+     * if p[i]==lb[i] g[i]<0; otherwise g[i]=0 
+     */
+    for(i=j=numactive=0, p_L2=jacTe_inf=0.0; i<m; ++i){
+      if(ub && p[i]==ub[i]){ ++numactive; if(jacTe[i]>0.0) ++j; }
+      else if(lb && p[i]==lb[i]){ ++numactive; if(jacTe[i]<0.0) ++j; }
+      else if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
+
+      diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
+      p_L2+=p[i]*p[i];
+    }
+    //p_L2=sqrt(p_L2);
+
+#if 0
+if(!(k%100)){
+  printf("Current estimate: ");
+  for(i=0; i<m; ++i)
+    printf("%.9g ", p[i]);
+  printf("-- errors %.9g %0.9g, #active %d [%d]\n", jacTe_inf, p_eL2, numactive, j);
+}
+#endif
+
+    /* check for convergence */
+    if(j==numactive && (jacTe_inf <= eps1)){
+      Dp_L2=0.0; /* no increment for p in this case */
+      stop=1;
+      break;
+    }
+
+   /* compute initial damping factor */
+    if(k==0){
+      if(!lb && !ub){ /* no bounds */
+        for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+          if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
+        mu=tau*tmp;
+      }
+      else 
+        mu=LM_CNST(0.5)*tau*p_eL2; /* use Kanzow's starting mu */
+    }
+
+    /* determine increment using a combination of adaptive damping, line search and projected gradient search */
+    while(1){
+      /* augment normal equations */
+      for(i=0; i<m; ++i)
+        jacTjac[i*m+i]+=mu;
+
+      /* solve augmented equations */
+#ifdef HAVE_LAPACK
+      /* 7 alternatives are available: LU, Cholesky + Cholesky with PLASMA, LDLt, 2 variants of QR decomposition and SVD.
+       * For matrices with dimensions of at least a few hundreds, the PLASMA implementation of Cholesky is the fastest.
+       * From the serial solvers, Cholesky is the fastest but might occasionally be inapplicable due to numerical round-off;
+       * QR is slower but more robust; SVD is the slowest but most robust; LU is quite robust but
+       * slower than LDLt; LDLt offers a good tradeoff between robustness and speed
+       */
+
+      issolved=AX_EQ_B_BK(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_BK;
+      //issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+      //issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;
+#ifdef HAVE_PLASMA
+      //issolved=AX_EQ_B_PLASMA_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_PLASMA_CHOL;
+#endif
+      //issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;
+      //issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;
+      //issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;
+
+#else
+      /* use the LU included with levmar */
+      issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
+#endif /* HAVE_LAPACK */
+
+      if(issolved){
+        for(i=0; i<m; ++i)
+          pDp[i]=p[i] + Dp[i];
+
+        /* compute p's new estimate and ||Dp||^2 */
+        BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
+        for(i=0, Dp_L2=0.0; i<m; ++i){
+          Dp[i]=tmp=pDp[i]-p[i];
+          Dp_L2+=tmp*tmp;
+        }
+        //Dp_L2=sqrt(Dp_L2);
+
+        if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
+          stop=2;
+          break;
+        }
+
+        if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
+          stop=4;
+          break;
+        }
+
+        if(!dscl){
+          (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
+        }
+        else{
+          for(i=m; i-->0; ) sp_pDp[i]=pDp[i]*dscl[i];
+          (*func)(sp_pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
+        }
+
+        /* ### hx=x-hx, pDp_eL2=||hx|| */
+#if 1
+        pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
+#else
+        for(i=0, pDp_eL2=0.0; i<n; ++i){ /* compute ||e(pDp)||_2 */
+          hx[i]=tmp=x[i]-hx[i];
+          pDp_eL2+=tmp*tmp;
+        }
+#endif
+        /* the following test ensures that the computation of pDp_eL2 has not overflowed.
+         * Such an overflow does no harm here, thus it is not signalled as an error
+         */
+        if(!LM_FINITE(pDp_eL2) && !LM_FINITE(VECNORM(hx, n))){
+          stop=7;
+          break;
+        }
+
+        if(pDp_eL2<=gamma*p_eL2){
+          for(i=0, dL=0.0; i<m; ++i)
+            dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
+
+#if 1
+          if(dL>0.0){
+            dF=p_eL2-pDp_eL2;
+            tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
+            tmp=LM_CNST(1.0)-tmp*tmp*tmp;
+            mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
+          }
+          else{
+            tmp=LM_CNST(0.1)*pDp_eL2; /* pDp_eL2 is the new p_eL2 */
+            mu=(mu>=tmp)? tmp : mu;
+          }
+#else
+
+          tmp=LM_CNST(0.1)*pDp_eL2; /* pDp_eL2 is the new p_eL2 */
+          mu=(mu>=tmp)? tmp : mu;
+#endif
+
+          nu=2;
+
+          for(i=0 ; i<m; ++i) /* update p's estimate */
+            p[i]=pDp[i];
+
+          for(i=0; i<n; ++i) /* update e and ||e||_2 */
+            e[i]=hx[i];
+          p_eL2=pDp_eL2;
+          ++nLMsteps;
+          gprevtaken=0;
+          break;
+        }
+        /* note that if the LM step is not taken, code falls through to the LM line search below */
+      }
+      else{
+
+      /* the augmented linear system could not be solved, increase mu */
+
+        mu*=nu;
+        nu2=nu<<1; // 2*nu;
+        if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
+          stop=5;
+          break;
+        }
+        nu=nu2;
+
+        for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+          jacTjac[i*m+i]=diag_jacTjac[i];
+
+        continue; /* solve again with increased nu */
+      }
+
+      /* if this point is reached, the LM step did not reduce the error;
+       * see if it is a descent direction
+       */
+
+      /* negate jacTe (i.e. g) & compute g^T * Dp */
+      for(i=0, jacTeDp=0.0; i<m; ++i){
+        jacTe[i]=-jacTe[i];
+        jacTeDp+=jacTe[i]*Dp[i];
+      }
+
+      if(jacTeDp<=-rho*pow(Dp_L2, LM_CNST(_POW_)/LM_CNST(2.0))){
+        /* Dp is a descent direction; do a line search along it */
+#if 1
+        /* use Schnabel's backtracking line search; it requires fewer "func" evaluations */
+        {
+        int mxtake, iretcd;
+        LM_REAL stepmx, steptl=LM_CNST(1e3)*(LM_REAL)sqrt(LM_REAL_EPSILON);
+
+        tmp=(LM_REAL)sqrt(p_L2); stepmx=LM_CNST(1e3)*( (tmp>=LM_CNST(1.0))? tmp : LM_CNST(1.0) );
+
+        LNSRCH(m, p, p_eL2, jacTe, Dp, alpha, pDp, &pDp_eL2, func, &fstate,
+               &mxtake, &iretcd, stepmx, steptl, dscl); /* NOTE: LNSRCH() updates hx */
+        if(iretcd!=0 || !LM_FINITE(pDp_eL2)) goto gradproj; /* rather inelegant but effective way to handle LNSRCH() failures... */
+        }
+#else
+        /* use the simpler (but slower!) line search described by Kanzow et al */
+        for(t=tini; t>tmin; t*=beta){
+          for(i=0; i<m; ++i)
+            pDp[i]=p[i] + t*Dp[i];
+          BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
+
+          if(!dscl){
+            (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + t*Dp */
+          }
+          else{
+            for(i=m; i-->0; ) sp_pDp[i]=pDp[i]*dscl[i];
+            (*func)(sp_pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + t*Dp */
+          }
+
+          /* compute ||e(pDp)||_2 */
+          /* ### hx=x-hx, pDp_eL2=||hx|| */
+#if 1
+          pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
+#else
+          for(i=0, pDp_eL2=0.0; i<n; ++i){
+            hx[i]=tmp=x[i]-hx[i];
+            pDp_eL2+=tmp*tmp;
+          }
+#endif /* ||e(pDp)||_2 */
+          if(!LM_FINITE(pDp_eL2)) goto gradproj; /* treat as line search failure */
+
+          //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + t*alpha*jacTeDp) break;
+          if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*t*alpha*jacTeDp) break;
+        }
+#endif /* line search alternatives */
+
+        ++nLSsteps;
+        gprevtaken=0;
+
+        /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2.
+         * These values are used below to update their corresponding variables 
+         */
+      }
+      else{
+        /* Note that this point can also be reached via a goto when LNSRCH() fails. */
+gradproj:
+
+        /* jacTe has been negated above. Being a descent direction, it is next used
+         * to make a projected gradient step
+         */
+
+        /* compute ||g|| */
+        for(i=0, tmp=0.0; i<m; ++i)
+          tmp+=jacTe[i]*jacTe[i];
+        tmp=(LM_REAL)sqrt(tmp);
+        tmp=LM_CNST(100.0)/(LM_CNST(1.0)+tmp);
+        t0=(tmp<=tini)? tmp : tini; /* guard against poor scaling & large steps; see (3.50) in C.T. Kelley's book */
+
+        /* if the previous step was along the gradient descent, try to use the t employed in that step */
+        for(t=(gprevtaken)? t : t0; t>tming; t*=beta){
+          for(i=0; i<m; ++i)
+            pDp[i]=p[i] - t*jacTe[i];
+          BOXPROJECT(pDp, lb, ub, m); /* project to feasible set */
+          for(i=0, Dp_L2=0.0; i<m; ++i){
+            Dp[i]=tmp=pDp[i]-p[i];
+            Dp_L2+=tmp*tmp;
+          }
+
+          if(!dscl){
+            (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p - t*g */
+          }
+          else{
+            for(i=m; i-->0; ) sp_pDp[i]=pDp[i]*dscl[i];
+            (*func)(sp_pDp, hx, m, n, adata); ++nfev; /* evaluate function at p - t*g */
+          }
+
+          /* compute ||e(pDp)||_2 */
+          /* ### hx=x-hx, pDp_eL2=||hx|| */
+#if 1
+          pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
+#else
+          for(i=0, pDp_eL2=0.0; i<n; ++i){
+            hx[i]=tmp=x[i]-hx[i];
+            pDp_eL2+=tmp*tmp;
+          }
+#endif
+          /* the following test ensures that the computation of pDp_eL2 has not overflowed.
+           * Such an overflow does no harm here, thus it is not signalled as an error
+           */
+          if(!LM_FINITE(pDp_eL2) && !LM_FINITE(VECNORM(hx, n))){
+            stop=7;
+            goto breaknested;
+          }
+
+          /* compute ||g^T * Dp||. Note that if pDp has not been altered by projection
+           * (i.e. BOXPROJECT), jacTeDp=-t*||g||^2
+           */
+          for(i=0, jacTeDp=0.0; i<m; ++i)
+            jacTeDp+=jacTe[i]*Dp[i];
+
+          if(gprevtaken && pDp_eL2<=p_eL2 + LM_CNST(2.0)*LM_CNST(0.99999)*jacTeDp){ /* starting t too small */
+            t=t0;
+            gprevtaken=0;
+            continue;
+          }
+          //if(LM_CNST(0.5)*pDp_eL2<=LM_CNST(0.5)*p_eL2 + alpha*jacTeDp) terminatePGLS;
+          if(pDp_eL2<=p_eL2 + LM_CNST(2.0)*alpha*jacTeDp) goto terminatePGLS;
+
+          //if(pDp_eL2<=p_eL2 - LM_CNST(2.0)*alpha/t*Dp_L2) goto terminatePGLS; // sufficient decrease condition proposed by Kelley in (5.13)
+        }
+        
+        /* if this point is reached then the gradient line search has failed */
+        gprevtaken=0;
+        break;
+
+terminatePGLS:
+
+        ++nPGsteps;
+        gprevtaken=1;
+        /* NOTE: new estimate for p is in pDp, associated error in hx and its norm in pDp_eL2 */
+      }
+
+      /* update using computed values */
+
+      for(i=0, Dp_L2=0.0; i<m; ++i){
+        tmp=pDp[i]-p[i];
+        Dp_L2+=tmp*tmp;
+      }
+      //Dp_L2=sqrt(Dp_L2);
+
+      if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
+        stop=2;
+        break;
+      }
+
+      for(i=0 ; i<m; ++i) /* update p's estimate */
+        p[i]=pDp[i];
+
+      for(i=0; i<n; ++i) /* update e and ||e||_2 */
+        e[i]=hx[i];
+      p_eL2=pDp_eL2;
+      break;
+    } /* inner loop */
+  }
+
+breaknested: /* NOTE: this point is also reached via an explicit goto! */
+
+  if(k>=itmax) stop=3;
+
+  for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
+    jacTjac[i*m+i]=diag_jacTjac[i];
+
+  if(info){
+    info[0]=init_p_eL2;
+    info[1]=p_eL2;
+    info[2]=jacTe_inf;
+    info[3]=Dp_L2;
+    for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
+      if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
+    info[4]=mu/tmp;
+    info[5]=(LM_REAL)k;
+    info[6]=(LM_REAL)stop;
+    info[7]=(LM_REAL)nfev;
+    info[8]=(LM_REAL)njev;
+    info[9]=(LM_REAL)nlss;
+  }
+
+  /* covariance matrix */
+  if(covar){
+    LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
+
+    if(dscl){ /* correct for the scaling */
+      for(i=m; i-->0; )
+        for(j=m; j-->0; )
+          covar[i*m+j]*=(dscl[i]*dscl[j]);
+    }
+  }
+                                                               
+  if(freework) free(work);
+
+#ifdef LINSOLVERS_RETAIN_MEMORY
+    if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
+#endif
+
+#if 0
+printf("%d LM steps, %d line search, %d projected gradient\n", nLMsteps, nLSsteps, nPGsteps);
+#endif
+
+  if(dscl){
+    /* scale final point and constraints */
+    for(i=0; i<m; ++i) p[i]*=dscl[i];
+    BOXSCALE(lb, ub, dscl, m, 0);
+    free(sp_pDp);
+  }
+
+  return (stop!=4 && stop!=7)?  k : LM_ERROR;
+}
+
+/* following struct & LMBC_DIF_XXX functions won't be necessary if a true secant
+ * version of LEVMAR_BC_DIF() is implemented...
+ */
+struct LMBC_DIF_DATA{
+  int ffdif; // nonzero if forward differencing is used
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
+  LM_REAL *hx, *hxx;
+  void *adata;
+  LM_REAL delta;
+};
+
+static void LMBC_DIF_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *data)
+{
+struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;
+
+  /* call user-supplied function passing it the user-supplied data */
+  (*(dta->func))(p, hx, m, n, dta->adata);
+}
+
+static void LMBC_DIF_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *data)
+{
+struct LMBC_DIF_DATA *dta=(struct LMBC_DIF_DATA *)data;
+
+  if(dta->ffdif){
+    /* evaluate user-supplied function at p */
+    (*(dta->func))(p, dta->hx, m, n, dta->adata);
+    LEVMAR_FDIF_FORW_JAC_APPROX(dta->func, p, dta->hx, dta->hxx, dta->delta, jac, m, n, dta->adata);
+  }
+  else
+    LEVMAR_FDIF_CENT_JAC_APPROX(dta->func, p, dta->hx, dta->hxx, dta->delta, jac, m, n, dta->adata);
+}
+
+
+/* No Jacobian version of the LEVMAR_BC_DER() function above: the Jacobian is approximated with 
+ * the aid of finite differences (forward or central, see the comment for the opts argument)
+ * Ideally, this function should be implemented with a secant approach. Currently, it just calls
+ * LEVMAR_BC_DER()
+ */
+int LEVMAR_BC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *dscl,      /* I: diagonal scaling constants. NULL implies no scaling */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[5],    /* I: opts[0-4] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
+                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
+                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
+                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
+                       * (but slower!) compared to the forward differences employed by default. 
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BC_DIF_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
+                      * Set to NULL if not needed
+                      */
+{
+struct LMBC_DIF_DATA data;
+int ret;
+
+  //fprintf(stderr, RCAT("\nWarning: current implementation of ", LEVMAR_BC_DIF) "() does not use a secant approach!\n\n");
+
+  data.ffdif=!opts || opts[4]>=0.0;
+
+  data.func=func;
+  data.hx=(LM_REAL *)malloc(2*n*sizeof(LM_REAL)); /* allocate a big chunk in one step */
+  if(!data.hx){
+    fprintf(stderr, LCAT(LEVMAR_BC_DIF, "(): memory allocation request failed\n"));
+    return LM_ERROR;
+  }
+  data.hxx=data.hx+n;
+  data.adata=adata;
+  data.delta=(opts)? FABS(opts[4]) : (LM_REAL)LM_DIFF_DELTA;
+
+  ret=LEVMAR_BC_DER(LMBC_DIF_FUNC, LMBC_DIF_JACF, p, x, m, n, lb, ub, dscl, itmax, opts, info, work, covar, (void *)&data);
+
+  if(info){ /* correct the number of function calls */
+    if(data.ffdif)
+      info[7]+=info[8]*(m+1); /* each Jacobian evaluation costs m+1 function calls */
+    else
+      info[7]+=info[8]*(2*m); /* each Jacobian evaluation costs 2*m function calls */
+  }
+
+  free(data.hx);
+
+  return ret;
+}
+
+/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef FUNC_STATE
+#undef LNSRCH
+#undef BOXPROJECT
+#undef BOXSCALE
+#undef LEVMAR_BOX_CHECK
+#undef VECNORM
+#undef LEVMAR_BC_DER
+#undef LMBC_DIF_DATA
+#undef LMBC_DIF_FUNC
+#undef LMBC_DIF_JACF
+#undef LEVMAR_BC_DIF
+#undef LEVMAR_FDIF_FORW_JAC_APPROX
+#undef LEVMAR_FDIF_CENT_JAC_APPROX
+#undef LEVMAR_COVAR
+#undef LEVMAR_TRANS_MAT_MAT_MULT
+#undef LEVMAR_L2NRMXMY
+#undef AX_EQ_B_LU
+#undef AX_EQ_B_CHOL
+#undef AX_EQ_B_PLASMA_CHOL
+#undef AX_EQ_B_QR
+#undef AX_EQ_B_QRLS
+#undef AX_EQ_B_SVD
+#undef AX_EQ_B_BK
Index: ThirdParty/levmar/src/lmblec.c
===================================================================
--- ThirdParty/levmar/src/lmblec.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmblec.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,87 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-06  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * combined box and linear equation constraints Levenberg-Marquardt nonlinear
+ * minimization. The same core code is used with appropriate #defines to derive
+ * single and double precision versions, see also lmblec_core.c
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+#include "misc.h"
+
+#ifndef HAVE_LAPACK
+
+#ifdef _MSC_VER
+#pragma message("Combined box and linearly constrained optimization requires LAPACK and was not compiled!")
+#else
+#warning Combined box and linearly constrained optimization requires LAPACK and was not compiled!
+#endif // _MSC_VER
+
+#else // LAPACK present
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define LM_REAL_MAX FLT_MAX
+#define LM_REAL_MIN -FLT_MAX
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "lmblec_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_REAL_MAX DBL_MAX
+#define LM_REAL_MIN -DBL_MAX
+#define LM_CNST(x) (x)
+
+#include "lmblec_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
+
+#endif /* HAVE_LAPACK */
Index: ThirdParty/levmar/src/lmblec_core.c
===================================================================
--- ThirdParty/levmar/src/lmblec_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmblec_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,413 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-06  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/*******************************************************************************
+ * This file implements combined box and linear equation constraints.
+ *
+ * Note that the algorithm implementing linearly constrained minimization does
+ * so by a change in parameters that transforms the original program into an
+ * unconstrained one. To employ the same idea for implementing box & linear
+ * constraints would require the transformation of box constraints on the
+ * original parameters to box constraints for the new parameter set. This
+ * being impossible, a different approach is used here for finding the minimum.
+ * The trick is to remove the box constraints by augmenting the function to
+ * be fitted with penalty terms and then solve the resulting problem (which
+ * involves linear constrains only) with the functions in lmlec.c
+ *
+ * More specifically, for the constraint a<=x[i]<=b to hold, the term C[i]=
+ * (2*x[i]-(a+b))/(b-a) should be within [-1, 1]. This is enforced by adding
+ * the penalty term w[i]*max((C[i])^2-1, 0) to the objective function, where
+ * w[i] is a large weight. In the case of constraints of the form a<=x[i],
+ * the term C[i]=a-x[i] has to be non positive, thus the penalty term is
+ * w[i]*max(C[i], 0). If x[i]<=b, C[i]=x[i]-b has to be non negative and
+ * the penalty is w[i]*max(C[i], 0). The derivatives needed for the Jacobian
+ * are as follows:
+ * For the constraint a<=x[i]<=b: 4*(2*x[i]-(a+b))/(b-a)^2 if x[i] not in [a, b],
+ *                                0 otherwise
+ * For the constraint a<=x[i]: -1 if x[i]<=a, 0 otherwise
+ * For the constraint x[i]<=b: 1 if b<=x[i], 0 otherwise
+ *
+ * Note that for the above to work, the weights w[i] should be large enough;
+ * depending on your minimization problem, the default values might need some
+ * tweaking (see arg "wghts" below).
+ *******************************************************************************/
+
+#ifndef LM_REAL // not included by lmblec.c
+#error This file should not be compiled directly!
+#endif
+
+
+#define __MAX__(x, y)   (((x)>=(y))? (x) : (y))
+#define __BC_WEIGHT__   LM_CNST(1E+04)
+
+#define __BC_INTERVAL__ 0
+#define __BC_LOW__      1
+#define __BC_HIGH__     2
+
+/* precision-specific definitions */
+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
+#define LMBLEC_DATA LM_ADD_PREFIX(lmblec_data)
+#define LMBLEC_FUNC LM_ADD_PREFIX(lmblec_func)
+#define LMBLEC_JACF LM_ADD_PREFIX(lmblec_jacf)
+#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
+#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
+#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)
+#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+
+struct LMBLEC_DATA{
+  LM_REAL *x, *lb, *ub, *w;
+  int *bctype;
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
+  void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
+  void *adata;
+};
+
+/* augmented measurements */
+static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
+{
+struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
+int nn;
+register int i, j, *typ;
+register LM_REAL *lb, *ub, *w, tmp;
+
+  nn=n-m;
+  lb=data->lb;
+  ub=data->ub;
+  w=data->w;
+  typ=data->bctype;
+  (*(data->func))(p, hx, m, nn, data->adata);
+
+  for(i=nn, j=0; i<n; ++i, ++j){
+    switch(typ[j]){
+      case __BC_INTERVAL__:
+        tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);
+        hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));
+      break;
+
+      case __BC_LOW__:
+        hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));
+      break;
+
+      case __BC_HIGH__:
+        hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));
+      break;
+    }
+  }
+}
+
+/* augmented Jacobian */
+static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
+{
+struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
+int nn, *typ;
+register int i, j;
+register LM_REAL *lb, *ub, *w, tmp;
+
+  nn=n-m;
+  lb=data->lb;
+  ub=data->ub;
+  w=data->w;
+  typ=data->bctype;
+  (*(data->jacf))(p, jac, m, nn, data->adata);
+
+  /* clear all extra rows */
+  for(i=nn*m; i<n*m; ++i)
+    jac[i]=0.0;
+
+  for(i=nn, j=0; i<n; ++i, ++j){
+    switch(typ[j]){
+      case __BC_INTERVAL__:
+        if(lb[j]<=p[j] && p[j]<=ub[j])
+          continue; // corresp. jac element already 0
+
+        /* out of interval */
+        tmp=ub[j]-lb[j];
+        tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);
+        jac[i*m+j]=w[j]*tmp;
+      break;
+
+      case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;
+        if(lb[j]<=p[j])
+          continue; // corresp. jac element already 0
+
+        /* smaller than lower bound */
+        jac[i*m+j]=-w[j];
+      break;
+
+      case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;
+        if(p[j]<=ub[j])
+          continue; // corresp. jac element already 0
+
+        /* greater than upper bound */
+        jac[i*m+j]=w[j];
+      break;
+    }
+  }
+}
+
+/* 
+ * This function seeks the parameter vector p that best describes the measurements
+ * vector x under box & linear constraints.
+ * More precisely, given a vector function  func : R^m --> R^n with n>=m,
+ * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
+ * e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;
+ * A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of
+ * the specified box and linear equation constraints.
+ * If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
+ * If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
+ *
+ * This function requires an analytic Jacobian. In case the latter is unavailable,
+ * use LEVMAR_BLEC_DIF() bellow
+ *
+ * Returns the number of iterations (>=0) if successful, LM_ERROR if failed
+ *
+ * For more details on the algorithm implemented by this function, please refer to
+ * the comments in the top of this file.
+ *
+ */
+int LEVMAR_BLEC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *A,         /* I: constraints matrix, kxm */
+  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
+  int k,              /* I: number of constraints (i.e. A's #rows) */
+  LM_REAL *wghts,     /* mx1 weights for penalty terms, defaults used if NULL */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
+                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMBLEC_DATA data;
+  int ret;
+  LM_REAL locinfo[LM_INFO_SZ];
+  register int i;
+
+  if(!jacf){
+    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEC_DER)
+      RCAT("().\nIf no such function is available, use ", LEVMAR_BLEC_DIF) RCAT("() rather than ", LEVMAR_BLEC_DER) "()\n");
+    return LM_ERROR;
+  }
+
+  if(!lb && !ub){
+    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DER, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
+          LEVMAR_LEC_DER) "() in this case!\n");
+    return LM_ERROR;
+  }
+
+  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
+    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
+    return LM_ERROR;
+  }
+
+  /* measurement vector needs to be extended by m */
+  if(x){ /* nonzero x */
+    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
+    if(!data.x){
+      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
+      return LM_ERROR;
+    }
+
+    for(i=0; i<n; ++i)
+      data.x[i]=x[i];
+    for(i=n; i<n+m; ++i)
+      data.x[i]=0.0;
+  }
+  else
+    data.x=NULL;
+
+  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
+  if(!data.w){
+    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
+    if(data.x) free(data.x);
+    return LM_ERROR;
+  }
+  data.bctype=(int *)(data.w+m);
+
+  /* note: at this point, one of lb, ub are not NULL */
+  for(i=0; i<m; ++i){
+    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
+    if(!lb) data.bctype[i]=__BC_HIGH__;
+    else if(!ub) data.bctype[i]=__BC_LOW__;
+    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
+    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
+    else data.bctype[i]=__BC_HIGH__;
+  }
+
+  data.lb=lb;
+  data.ub=ub;
+  data.func=func;
+  data.jacf=jacf;
+  data.adata=adata;
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
+  ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
+
+  if(data.x) free(data.x);
+  free(data.w);
+
+  return ret;
+}
+
+/* Similar to the LEVMAR_BLEC_DER() function above, except that the Jacobian is approximated
+ * with the aid of finite differences (forward or central, see the comment for the opts argument)
+ */
+int LEVMAR_BLEC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *A,         /* I: constraints matrix, kxm */
+  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
+  int k,              /* I: number of constraints (i.e. A's #rows) */
+  LM_REAL *wghts,     /* mx1 weights for penalty terms, defaults used if NULL */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
+                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
+                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
+                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
+                       * (but slower!) compared to the forward differences employed by default. 
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMBLEC_DATA data;
+  int ret;
+  register int i;
+  LM_REAL locinfo[LM_INFO_SZ];
+
+  if(!lb && !ub){
+    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DIF, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
+          LEVMAR_LEC_DIF) "() in this case!\n");
+    return LM_ERROR;
+  }
+
+  if(!LEVMAR_BOX_CHECK(lb, ub, m)){
+    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
+    return LM_ERROR;
+  }
+
+  /* measurement vector needs to be extended by m */
+  if(x){ /* nonzero x */
+    data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
+    if(!data.x){
+      fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
+      return LM_ERROR;
+    }
+
+    for(i=0; i<n; ++i)
+      data.x[i]=x[i];
+    for(i=n; i<n+m; ++i)
+      data.x[i]=0.0;
+  }
+  else
+    data.x=NULL;
+
+  data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
+  if(!data.w){
+    fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
+    if(data.x) free(data.x);
+    return LM_ERROR;
+  }
+  data.bctype=(int *)(data.w+m);
+
+  /* note: at this point, one of lb, ub are not NULL */
+  for(i=0; i<m; ++i){
+    data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
+    if(!lb) data.bctype[i]=__BC_HIGH__;
+    else if(!ub) data.bctype[i]=__BC_LOW__;
+    else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
+    else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
+    else data.bctype[i]=__BC_HIGH__;
+  }
+
+  data.lb=lb;
+  data.ub=ub;
+  data.func=func;
+  data.jacf=NULL;
+  data.adata=adata;
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
+  ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
+
+  if(data.x) free(data.x);
+  free(data.w);
+
+  return ret;
+}
+
+/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef LEVMAR_BOX_CHECK
+#undef LMBLEC_DATA
+#undef LMBLEC_FUNC
+#undef LMBLEC_JACF
+#undef LEVMAR_COVAR
+#undef LEVMAR_LEC_DER
+#undef LEVMAR_LEC_DIF
+#undef LEVMAR_BLEC_DER
+#undef LEVMAR_BLEC_DIF
Index: ThirdParty/levmar/src/lmbleic.c
===================================================================
--- ThirdParty/levmar/src/lmbleic.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmbleic.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,89 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2009  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/*******************************************************************************
+ * Wrappers for linear inequality constrained Levenberg-Marquardt minimization.
+ * The same core code is used with appropriate #defines to derive single and
+ * double precision versions, see also lmbleic_core.c
+ *******************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+#include "misc.h"
+
+
+#ifndef HAVE_LAPACK
+
+#ifdef _MSC_VER
+#pragma message("Linear inequalities constrained optimization requires LAPACK and was not compiled!")
+#else
+#warning Linear inequalities constrained optimization requires LAPACK and was not compiled!
+#endif // _MSC_VER
+
+#else // LAPACK present
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define LM_REAL_MAX FLT_MAX
+#define LM_REAL_MIN -FLT_MAX
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "lmbleic_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_REAL_MAX DBL_MAX
+#define LM_REAL_MIN -DBL_MAX
+#define LM_CNST(x) (x)
+
+#include "lmbleic_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_MAX
+#undef LM_REAL_MIN
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
+
+#endif /* HAVE_LAPACK */
+
Index: ThirdParty/levmar/src/lmbleic_core.c
===================================================================
--- ThirdParty/levmar/src/lmbleic_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmbleic_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,506 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2009  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LM_REAL // not included by lmbleic.c
+#error This file should not be compiled directly!
+#endif
+
+
+/* precision-specific definitions */
+#define LMBLEIC_DATA LM_ADD_PREFIX(lmbleic_data)
+#define LMBLEIC_ELIM LM_ADD_PREFIX(lmbleic_elim)
+#define LMBLEIC_FUNC LM_ADD_PREFIX(lmbleic_func)
+#define LMBLEIC_JACF LM_ADD_PREFIX(lmbleic_jacf)
+#define LEVMAR_BLEIC_DER LM_ADD_PREFIX(levmar_bleic_der)
+#define LEVMAR_BLEIC_DIF LM_ADD_PREFIX(levmar_bleic_dif)
+#define LEVMAR_BLIC_DER LM_ADD_PREFIX(levmar_blic_der)
+#define LEVMAR_BLIC_DIF LM_ADD_PREFIX(levmar_blic_dif)
+#define LEVMAR_LEIC_DER LM_ADD_PREFIX(levmar_leic_der)
+#define LEVMAR_LEIC_DIF LM_ADD_PREFIX(levmar_leic_dif)
+#define LEVMAR_LIC_DER LM_ADD_PREFIX(levmar_lic_der)
+#define LEVMAR_LIC_DIF LM_ADD_PREFIX(levmar_lic_dif)
+#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)
+#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)
+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
+
+struct LMBLEIC_DATA{
+  LM_REAL *jac;
+  int nineqcnstr; // #inequality constraints
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
+  void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
+  void *adata;
+};
+
+
+/* wrapper ensuring that the user-supplied function is called with the right number of variables (i.e. m) */
+static void LMBLEIC_FUNC(LM_REAL *pext, LM_REAL *hx, int mm, int n, void *adata)
+{
+struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata;
+int m;
+
+  m=mm-data->nineqcnstr;
+  (*(data->func))(pext, hx, m, n, data->adata);
+}
+
+
+/* wrapper for computing the Jacobian at pext. The Jacobian is nxmm */
+static void LMBLEIC_JACF(LM_REAL *pext, LM_REAL *jacext, int mm, int n, void *adata)
+{
+struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata;
+int m;
+register int i, j;
+LM_REAL *jac, *jacim, *jacextimm;
+
+  m=mm-data->nineqcnstr;
+  jac=data->jac;
+
+  (*(data->jacf))(pext, jac, m, n, data->adata);
+
+  for(i=0; i<n; ++i){
+    jacextimm=jacext+i*mm;
+    jacim=jac+i*m;
+    for(j=0; j<m; ++j)
+      jacextimm[j]=jacim[j]; //jacext[i*mm+j]=jac[i*m+j];
+
+    for(j=m; j<mm; ++j)
+      jacextimm[j]=0.0; //jacext[i*mm+j]=0.0;
+  }
+}
+
+
+/* 
+ * This function is similar to LEVMAR_DER except that the minimization is
+ * performed subject to the box constraints lb[i]<=p[i]<=ub[i], the linear
+ * equation constraints A*p=b, A being k1xm, b k1x1, and the linear inequality
+ * constraints C*p>=d, C being k2xm, d k2x1. 
+ *
+ * The inequalities are converted to equations by introducing surplus variables,
+ * i.e. c^T*p >= d becomes c^T*p - y = d, with y>=0. To transform all inequalities
+ * to equations, a total of k2 surplus variables are introduced; a problem with only
+ * box and linear constraints results then and is solved with LEVMAR_BLEC_DER()
+ * Note that opposite direction inequalities should be converted to the desired
+ * direction by negating, i.e. c^T*p <= d becomes -c^T*p >= -d
+ *
+ * This function requires an analytic Jacobian. In case the latter is unavailable,
+ * use LEVMAR_BLEIC_DIF() bellow
+ *
+ */
+int LEVMAR_BLEIC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *A,         /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */
+  LM_REAL *b,         /* I: right hand constraints vector, k1x1 */
+  int k1,             /* I: number of constraints (i.e. A's #rows) */
+  LM_REAL *C,         /* I: inequality constraints matrix, k2xm */
+  LM_REAL *d,         /* I: right hand constraints vector, k2x1 */
+  int k2,             /* I: number of inequality constraints (i.e. C's #rows) */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
+                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BLEIC_DER_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMBLEIC_DATA data;
+  LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables;
+                                                 pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm
+                                               */
+  LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables
+  int mm, ret, k12;
+  register int i, j, ii;
+  register LM_REAL tmp;
+  LM_REAL locinfo[LM_INFO_SZ];
+
+  if(!jacf){
+    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEIC_DER)
+      RCAT("().\nIf no such function is available, use ", LEVMAR_BLEIC_DIF) RCAT("() rather than ", LEVMAR_BLEIC_DER) "()\n");
+    return LM_ERROR;
+  }
+
+  if(!C || !d){
+    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DER, "(): missing inequality constraints, use "), LEVMAR_BLEC_DER) "() in this case!\n");
+    return LM_ERROR;
+  }
+
+  if(!A || !b) k1=0; // sanity check
+
+  mm=m+k2;
+
+  if(n<m-k1){
+    fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m);
+    return LM_ERROR;
+  }
+
+  k12=k1+k2;
+  ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + n*m + (covar? mm*mm : 0))*sizeof(LM_REAL));
+  if(!ptr){
+    fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): memory allocation request failed\n"));
+    return LM_ERROR;
+  }
+  pext=ptr;
+  lbext=pext+mm;
+  ubext=lbext+mm;
+  Aext=ubext+mm;
+  bext=Aext+k12*mm;
+  data.jac=bext+k12;
+  covext=covar? data.jac+n*m : NULL;
+  data.nineqcnstr=k2;
+  data.func=func;
+  data.jacf=jacf;
+  data.adata=adata;
+
+  /* compute y s.t. C*p - y=d, i.e. y=C*p-d.
+   * y is stored in the last k2 elements of pext
+   */
+  for(i=0; i<k2; ++i){
+    for(j=0, tmp=0.0; j<m; ++j)
+      tmp+=C[i*m+j]*p[j];
+    pext[j=i+m]=tmp-d[i];
+
+    /* surplus variables must be >=0 */
+    lbext[j]=0.0;
+    ubext[j]=LM_REAL_MAX;
+  }
+  /* set the first m elements of pext equal to p */
+  for(i=0; i<m; ++i){
+    pext[i]=p[i];
+    lbext[i]=lb? lb[i] : LM_REAL_MIN;
+    ubext[i]=ub? ub[i] : LM_REAL_MAX;
+  }
+
+  /* setup the constraints matrix */
+  /* original linear equation constraints */
+  for(i=0; i<k1; ++i){
+    for(j=0; j<m; ++j)
+      Aext[i*mm+j]=A[i*m+j];
+
+    for(j=m; j<mm; ++j)
+      Aext[i*mm+j]=0.0;
+
+    bext[i]=b[i];
+  }
+  /* linear equation constraints resulting from surplus variables */
+  for(i=0, ii=k1; i<k2; ++i, ++ii){
+    for(j=0; j<m; ++j)
+      Aext[ii*mm+j]=C[i*m+j];
+
+    for(j=m; j<mm; ++j)
+      Aext[ii*mm+j]=0.0;
+
+    Aext[ii*mm+m+i]=-1.0;
+
+    bext[ii]=d[i];
+  }
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DER() is called with non-null info */
+  /* note that the default weights for the penalty terms are being used below */
+  ret=LEVMAR_BLEC_DER(LMBLEIC_FUNC, LMBLEIC_JACF, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data);
+
+  /* copy back the minimizer */
+  for(i=0; i<m; ++i)
+    p[i]=pext[i];
+
+#if 0
+printf("Surplus variables for the minimizer:\n");
+for(i=m; i<mm; ++i)
+  printf("%g ", pext[i]);
+printf("\n\n");
+#endif
+
+  if(covar){
+    for(i=0; i<m; ++i){
+      for(j=0; j<m; ++j)
+        covar[i*m+j]=covext[i*mm+j];
+    }
+  }
+
+  free(ptr);
+
+  return ret;
+}
+
+/* Similar to the LEVMAR_BLEIC_DER() function above, except that the Jacobian is approximated
+ * with the aid of finite differences (forward or central, see the comment for the opts argument)
+ */
+int LEVMAR_BLEIC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *lb,        /* I: vector of lower bounds. If NULL, no lower bounds apply */
+  LM_REAL *ub,        /* I: vector of upper bounds. If NULL, no upper bounds apply */
+  LM_REAL *A,         /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */
+  LM_REAL *b,         /* I: right hand constraints vector, k1x1 */
+  int k1,             /* I: number of constraints (i.e. A's #rows) */
+  LM_REAL *C,         /* I: inequality constraints matrix, k2xm */
+  LM_REAL *d,         /* I: right hand constraints vector, k2x1 */
+  int k2,             /* I: number of inequality constraints (i.e. C's #rows) */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
+                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
+                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
+                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
+                       * (but slower!) compared to the forward differences employed by default. 
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_BLEIC_DIF_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMBLEIC_DATA data;
+  LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables;
+                                                 pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm
+                                               */
+  LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables
+  int mm, ret, k12;
+  register int i, j, ii;
+  register LM_REAL tmp;
+  LM_REAL locinfo[LM_INFO_SZ];
+
+  if(!C || !d){
+    fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DIF, "(): missing inequality constraints, use "), LEVMAR_BLEC_DIF) "() in this case!\n");
+    return LM_ERROR;
+  }
+  if(!A || !b) k1=0; // sanity check
+
+  mm=m+k2;
+
+  if(n<m-k1){
+    fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m);
+    return LM_ERROR;
+  }
+
+  k12=k1+k2;
+  ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + (covar? mm*mm : 0))*sizeof(LM_REAL));
+  if(!ptr){
+    fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): memory allocation request failed\n"));
+    return LM_ERROR;
+  }
+  pext=ptr;
+  lbext=pext+mm;
+  ubext=lbext+mm;
+  Aext=ubext+mm;
+  bext=Aext+k12*mm;
+  data.jac=NULL;
+  covext=covar? bext+k12 : NULL;
+  data.nineqcnstr=k2;
+  data.func=func;
+  data.jacf=NULL;
+  data.adata=adata;
+
+  /* compute y s.t. C*p - y=d, i.e. y=C*p-d.
+   * y is stored in the last k2 elements of pext
+   */
+  for(i=0; i<k2; ++i){
+    for(j=0, tmp=0.0; j<m; ++j)
+      tmp+=C[i*m+j]*p[j];
+    pext[j=i+m]=tmp-d[i];
+
+    /* surplus variables must be >=0 */
+    lbext[j]=0.0;
+    ubext[j]=LM_REAL_MAX;
+  }
+  /* set the first m elements of pext equal to p */
+  for(i=0; i<m; ++i){
+    pext[i]=p[i];
+    lbext[i]=lb? lb[i] : LM_REAL_MIN; 
+    ubext[i]=ub? ub[i] : LM_REAL_MAX;
+  }
+
+  /* setup the constraints matrix */
+  /* original linear equation constraints */
+  for(i=0; i<k1; ++i){
+    for(j=0; j<m; ++j)
+      Aext[i*mm+j]=A[i*m+j];
+
+    for(j=m; j<mm; ++j)
+      Aext[i*mm+j]=0.0;
+
+    bext[i]=b[i];
+  }
+  /* linear equation constraints resulting from surplus variables */
+  for(i=0, ii=k1; i<k2; ++i, ++ii){
+    for(j=0; j<m; ++j)
+      Aext[ii*mm+j]=C[i*m+j];
+
+    for(j=m; j<mm; ++j)
+      Aext[ii*mm+j]=0.0;
+
+    Aext[ii*mm+m+i]=-1.0;
+
+    bext[ii]=d[i];
+  }
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DIF() is called with non-null info */
+  /* note that the default weights for the penalty terms are being used below */
+  ret=LEVMAR_BLEC_DIF(LMBLEIC_FUNC, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data);
+
+  /* copy back the minimizer */
+  for(i=0; i<m; ++i)
+    p[i]=pext[i];
+
+#if 0
+printf("Surplus variables for the minimizer:\n");
+for(i=m; i<mm; ++i)
+  printf("%g ", pext[i]);
+printf("\n\n");
+#endif
+
+  if(covar){
+    for(i=0; i<m; ++i){
+      for(j=0; j<m; ++j)
+        covar[i*m+j]=covext[i*mm+j];
+    }
+  }
+
+  free(ptr);
+
+  return ret;
+}
+
+
+/* convenience wrappers to LEVMAR_BLEIC_DER/LEVMAR_BLEIC_DIF */
+
+/* box & linear inequality constraints */
+int LEVMAR_BLIC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *lb, LM_REAL *ub,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+int LEVMAR_BLIC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *lb, LM_REAL *ub,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DIF(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+/* linear equation & inequality constraints */
+int LEVMAR_LEIC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *A, LM_REAL *b, int k1,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+int LEVMAR_LEIC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *A, LM_REAL *b, int k1,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+/* linear inequality constraints */
+int LEVMAR_LIC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+int LEVMAR_LIC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+  LM_REAL *p, LM_REAL *x, int m, int n,
+  LM_REAL *C, LM_REAL *d, int k2,
+  int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
+{
+  return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
+}
+
+/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef LMBLEIC_DATA
+#undef LMBLEIC_ELIM
+#undef LMBLEIC_FUNC
+#undef LMBLEIC_JACF
+#undef LEVMAR_FDIF_FORW_JAC_APPROX
+#undef LEVMAR_COVAR
+#undef LEVMAR_TRANS_MAT_MAT_MULT
+#undef LEVMAR_BLEIC_DER
+#undef LEVMAR_BLEIC_DIF
+#undef LEVMAR_BLIC_DER
+#undef LEVMAR_BLIC_DIF
+#undef LEVMAR_LEIC_DER
+#undef LEVMAR_LEIC_DIF
+#undef LEVMAR_LIC_DER
+#undef LEVMAR_LIC_DIF
+#undef LEVMAR_BLEC_DER
+#undef LEVMAR_BLEC_DIF
Index: ThirdParty/levmar/src/lmdemo.c
===================================================================
--- ThirdParty/levmar/src/lmdemo.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmdemo.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,1230 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Demonstration driver program for the Levenberg - Marquardt minimization
+//  algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * Levenberg-Marquardt minimization demo driver. Only the double precision versions
+ * are tested here. See the Meyer case for an example of verifying the Jacobian 
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+
+#ifndef LM_DBL_PREC
+#error Demo program assumes that levmar has been compiled with double precision, see LM_DBL_PREC!
+#endif
+
+
+/* Sample functions to be minimized with LM and their Jacobians.
+ * More test functions at http://www.csit.fsu.edu/~burkardt/f_src/test_nls/test_nls.html
+ * Check also the CUTE problems collection at ftp://ftp.numerical.rl.ac.uk/pub/cute/;
+ * CUTE is searchable through http://numawww.mathematik.tu-darmstadt.de:8081/opti/select.html
+ * CUTE problems can also be solved through the AMPL web interface at http://www.ampl.com/TRYAMPL/startup.html
+ *
+ * Nonlinear optimization models in AMPL can be found at http://www.princeton.edu/~rvdb/ampl/nlmodels/
+ */
+
+#define ROSD 105.0
+
+/* Rosenbrock function, global minimum at (1, 1) */
+void ros(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; ++i)
+    x[i]=((1.0-p[0])*(1.0-p[0]) + ROSD*(p[1]-p[0]*p[0])*(p[1]-p[0]*p[0]));
+}
+
+void jacros(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+
+  for(i=j=0; i<n; ++i){
+    jac[j++]=(-2 + 2*p[0]-4*ROSD*(p[1]-p[0]*p[0])*p[0]);
+    jac[j++]=(2*ROSD*(p[1]-p[0]*p[0]));
+  }
+}
+
+
+#define MODROSLAM 1E02
+/* Modified Rosenbrock problem, global minimum at (1, 1) */
+void modros(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; i+=3){
+    x[i]=10*(p[1]-p[0]*p[0]);
+	  x[i+1]=1.0-p[0];
+	  x[i+2]=MODROSLAM;
+  }
+}
+
+void jacmodros(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+
+  for(i=j=0; i<n; i+=3){
+    jac[j++]=-20.0*p[0];
+	  jac[j++]=10.0;
+
+	  jac[j++]=-1.0;
+	  jac[j++]=0.0;
+
+	  jac[j++]=0.0;
+	  jac[j++]=0.0;
+  }
+}
+
+
+/* Powell's function, minimum at (0, 0) */
+void powell(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; i+=2){
+    x[i]=p[0];
+    x[i+1]=10.0*p[0]/(p[0]+0.1) + 2*p[1]*p[1];
+  }
+}
+
+void jacpowell(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+
+  for(i=j=0; i<n; i+=2){
+    jac[j++]=1.0;
+    jac[j++]=0.0;
+
+    jac[j++]=1.0/((p[0]+0.1)*(p[0]+0.1));
+    jac[j++]=4.0*p[1];
+  }
+}
+
+/* Wood's function, minimum at (1, 1, 1, 1) */
+void wood(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; i+=6){
+    x[i]=10.0*(p[1] - p[0]*p[0]);
+    x[i+1]=1.0 - p[0];
+    x[i+2]=sqrt(90.0)*(p[3] - p[2]*p[2]);
+    x[i+3]=1.0 - p[2];
+    x[i+4]=sqrt(10.0)*(p[1]+p[3] - 2.0);
+    x[i+5]=(p[1] - p[3])/sqrt(10.0);
+  }
+}
+
+/* Meyer's (reformulated) problem, minimum at (2.48, 6.18, 3.45) */
+void meyer(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double ui;
+
+	for(i=0; i<n; ++i){
+		ui=0.45+0.05*i;
+		x[i]=p[0]*exp(10.0*p[1]/(ui+p[2]) - 13.0);
+	}
+}
+
+void jacmeyer(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double ui, tmp;
+
+  for(i=j=0; i<n; ++i){
+	  ui=0.45+0.05*i;
+	  tmp=exp(10.0*p[1]/(ui+p[2]) - 13.0);
+
+	  jac[j++]=tmp;
+	  jac[j++]=10.0*p[0]*tmp/(ui+p[2]);
+	  jac[j++]=-10.0*p[0]*p[1]*tmp/((ui+p[2])*(ui+p[2]));
+  }
+}
+
+/* Osborne's problem, minimum at (0.3754, 1.9358, -1.4647, 0.0129, 0.0221) */
+void osborne(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double t;
+
+	for(i=0; i<n; ++i){
+    t=10*i;
+    x[i]=p[0] + p[1]*exp(-p[3]*t) + p[2]*exp(-p[4]*t);
+	}
+}
+
+void jacosborne(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double t, tmp1, tmp2;
+
+  for(i=j=0; i<n; ++i){
+    t=10*i;
+	  tmp1=exp(-p[3]*t);
+    tmp2=exp(-p[4]*t);
+
+	  jac[j++]=1.0;
+	  jac[j++]=tmp1;
+	  jac[j++]=tmp2;
+    jac[j++]=-p[1]*t*tmp1;
+    jac[j++]=-p[2]*t*tmp2;
+  }
+}
+
+/* helical valley function, minimum at (1.0, 0.0, 0.0) */
+#ifndef M_PI
+#define M_PI   3.14159265358979323846  /* pi */
+#endif
+
+void helval(double *p, double *x, int m, int n, void *data)
+{
+double theta;
+
+  if(p[0]<0.0)
+     theta=atan(p[1]/p[0])/(2.0*M_PI) + 0.5;
+  else if(0.0<p[0])
+     theta=atan(p[1]/p[0])/(2.0*M_PI);
+  else 
+    theta=(p[1]>=0)? 0.25 : -0.25;
+
+  x[0]=10.0*(p[2] - 10.0*theta);
+  x[1]=10.0*(sqrt(p[0]*p[0] + p[1]*p[1]) - 1.0);
+  x[2]=p[2];
+}
+
+void jachelval(double *p, double *jac, int m, int n, void *data)
+{
+register int i=0;
+double tmp;
+
+  tmp=p[0]*p[0] + p[1]*p[1];
+
+  jac[i++]=50.0*p[1]/(M_PI*tmp);
+  jac[i++]=-50.0*p[0]/(M_PI*tmp);
+  jac[i++]=10.0;
+
+  jac[i++]=10.0*p[0]/sqrt(tmp);
+  jac[i++]=10.0*p[1]/sqrt(tmp);
+  jac[i++]=0.0;
+
+  jac[i++]=0.0;
+  jac[i++]=0.0;
+  jac[i++]=1.0;
+}
+
+/* Boggs - Tolle problem 3 (linearly constrained), minimum at (-0.76744, 0.25581, 0.62791, -0.11628, 0.25581)
+ * constr1: p[0] + 3*p[1] = 0;
+ * constr2: p[2] + p[3] - 2*p[4] = 0;
+ * constr3: p[1] - p[4] = 0;
+ */
+void bt3(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double t1, t2, t3, t4;
+
+  t1=p[0]-p[1];
+  t2=p[1]+p[2]-2.0;
+  t3=p[3]-1.0;
+  t4=p[4]-1.0;
+
+  for(i=0; i<n; ++i)
+    x[i]=t1*t1 + t2*t2 + t3*t3 + t4*t4;
+}
+
+void jacbt3(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double t1, t2, t3, t4;
+
+  t1=p[0]-p[1];
+  t2=p[1]+p[2]-2.0;
+  t3=p[3]-1.0;
+  t4=p[4]-1.0;
+
+  for(i=j=0; i<n; ++i){
+    jac[j++]=2.0*t1;
+    jac[j++]=2.0*(t2-t1);
+    jac[j++]=2.0*t2;
+    jac[j++]=2.0*t3;
+    jac[j++]=2.0*t4;
+  }
+}
+
+/* Hock - Schittkowski problem 28 (linearly constrained), minimum at (0.5, -0.5, 0.5)
+ * constr1: p[0] + 2*p[1] + 3*p[2] = 1;
+ */
+void hs28(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double t1, t2;
+
+  t1=p[0]+p[1];
+  t2=p[1]+p[2];
+
+  for(i=0; i<n; ++i)
+    x[i]=t1*t1 + t2*t2;
+}
+
+void jachs28(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double t1, t2;
+
+  t1=p[0]+p[1];
+  t2=p[1]+p[2];
+
+  for(i=j=0; i<n; ++i){
+    jac[j++]=2.0*t1;
+    jac[j++]=2.0*(t1+t2);
+    jac[j++]=2.0*t2;
+  }
+}
+
+/* Hock - Schittkowski problem 48 (linearly constrained), minimum at (1.0, 1.0, 1.0, 1.0, 1.0)
+ * constr1: sum {i in 0..4} p[i] = 5;
+ * constr2: p[2] - 2*(p[3]+p[4]) = -3;
+ */
+void hs48(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double t1, t2, t3;
+
+  t1=p[0]-1.0;
+  t2=p[1]-p[2];
+  t3=p[3]-p[4];
+
+  for(i=0; i<n; ++i)
+    x[i]=t1*t1 + t2*t2 + t3*t3;
+}
+
+void jachs48(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double t1, t2, t3;
+
+  t1=p[0]-1.0;
+  t2=p[1]-p[2];
+  t3=p[3]-p[4];
+
+  for(i=j=0; i<n; ++i){
+    jac[j++]=2.0*t1;
+    jac[j++]=2.0*t2;
+    jac[j++]=-2.0*t2;
+    jac[j++]=2.0*t3;
+    jac[j++]=-2.0*t3;
+  }
+}
+
+/* Hock - Schittkowski problem 51 (linearly constrained), minimum at (1.0, 1.0, 1.0, 1.0, 1.0)
+ * constr1: p[0] + 3*p[1] = 4;
+ * constr2: p[2] + p[3] - 2*p[4] = 0;
+ * constr3: p[1] - p[4] = 0;
+ */
+void hs51(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+double t1, t2, t3, t4;
+
+  t1=p[0]-p[1];
+  t2=p[1]+p[2]-2.0;
+  t3=p[3]-1.0;
+  t4=p[4]-1.0;
+
+  for(i=0; i<n; ++i)
+    x[i]=t1*t1 + t2*t2 + t3*t3 + t4*t4;
+}
+
+void jachs51(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+double t1, t2, t3, t4;
+
+  t1=p[0]-p[1];
+  t2=p[1]+p[2]-2.0;
+  t3=p[3]-1.0;
+  t4=p[4]-1.0;
+
+  for(i=j=0; i<n; ++i){
+    jac[j++]=2.0*t1;
+    jac[j++]=2.0*(t2-t1);
+    jac[j++]=2.0*t2;
+    jac[j++]=2.0*t3;
+    jac[j++]=2.0*t4;
+  }
+}
+
+/* Hock - Schittkowski problem 01 (box constrained), minimum at (1.0, 1.0)
+ * constr1: p[1]>=-1.5;
+ */
+void hs01(double *p, double *x, int m, int n, void *data)
+{
+double t;
+
+  t=p[0]*p[0];
+  x[0]=10.0*(p[1]-t);
+  x[1]=1.0-p[0];
+}
+
+void jachs01(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=-20.0*p[0];
+  jac[j++]=10.0;
+
+  jac[j++]=-1.0;
+  jac[j++]=0.0;
+}
+
+/* Hock - Schittkowski MODIFIED problem 21 (box constrained), minimum at (2.0, 0.0)
+ * constr1: 2 <= p[0] <=50;
+ * constr2: -50 <= p[1] <=50;
+ *
+ * Original HS21 has the additional constraint 10*p[0] - p[1] >= 10; which is inactive
+ * at the solution, so it is dropped here.
+ */
+void hs21(double *p, double *x, int m, int n, void *data)
+{
+  x[0]=p[0]/10.0;
+  x[1]=p[1];
+}
+
+void jachs21(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=0.1;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+}
+
+/* Problem hatfldb (box constrained), minimum at (0.947214, 0.8, 0.64, 0.4096)
+ * constri: p[i]>=0.0; (i=1..4)
+ * constr5: p[1]<=0.8;
+ */
+void hatfldb(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  x[0]=p[0]-1.0;
+
+  for(i=1; i<m; ++i)
+     x[i]=p[i-1]-sqrt(p[i]);
+}
+
+void jachatfldb(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=1.0;
+  jac[j++]=-0.5/sqrt(p[1]);
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=-0.5/sqrt(p[2]);
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=-0.5/sqrt(p[3]);
+}
+
+/* Problem hatfldc (box constrained), minimum at (1.0, 1.0, 1.0, 1.0)
+ * constri: p[i]>=0.0; (i=1..4)
+ * constri+4: p[i]<=10.0; (i=1..4)
+ */
+void hatfldc(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  x[0]=p[0]-1.0;
+
+  for(i=1; i<m-1; ++i)
+     x[i]=p[i-1]-sqrt(p[i]);
+
+  x[m-1]=p[m-1]-1.0;
+}
+
+void jachatfldc(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=1.0;
+  jac[j++]=-0.5/sqrt(p[1]);
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=-0.5/sqrt(p[2]);
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+}
+
+/* Hock - Schittkowski (modified #1) problem 52 (box/linearly constrained), minimum at (-0.09, 0.03, 0.25, -0.19, 0.03)
+ * constr1: p[0] + 3*p[1] = 0;
+ * constr2: p[2] +   p[3] - 2*p[4] = 0;
+ * constr3: p[1] -   p[4] = 0;
+ *
+ * To the above 3 constraints, we add the following 5:
+ * constr4: -0.09 <= p[0];
+ * constr5:   0.0 <= p[1] <= 0.3;
+ * constr6:          p[2] <= 0.25;
+ * constr7:  -0.2 <= p[3] <= 0.3;
+ * constr8:   0.0 <= p[4] <= 0.3;
+ *
+ */
+void mod1hs52(double *p, double *x, int m, int n, void *data)
+{
+  x[0]=4.0*p[0]-p[1];
+  x[1]=p[1]+p[2]-2.0;
+  x[2]=p[3]-1.0;
+  x[3]=p[4]-1.0;
+}
+
+void jacmod1hs52(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=4.0;
+  jac[j++]=-1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+}
+
+
+/* Hock - Schittkowski (modified #2) problem 52 (linear inequality constrained), minimum at (0.5, 2.0, 0.0, 1.0, 1.0)
+ * A fifth term [(p[0]-0.5)^2] is added to the objective function and 
+ * the equality contraints are replaced by the following inequalities:
+ * constr1: p[0] + 3*p[1] >= -1.0;
+ * constr2: p[2] +   p[3] - 2*p[4] >= -2.0;
+ * constr3: p[1] -   p[4] <= 7.0;
+ *
+ *
+ */
+void mod2hs52(double *p, double *x, int m, int n, void *data)
+{
+  x[0]=4.0*p[0]-p[1];
+  x[1]=p[1]+p[2]-2.0;
+  x[2]=p[3]-1.0;
+  x[3]=p[4]-1.0;
+  x[4]=p[0]-0.5;
+}
+
+void jacmod2hs52(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=4.0;
+  jac[j++]=-1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+}
+
+/* Schittkowski (modified) problem 235 (box/linearly constrained), minimum at (-1.725, 2.9, 0.725)
+ * constr1: p[0] + p[2] = -1.0;
+ *
+ * To the above constraint, we add the following 2:
+ * constr2: p[1] - 4*p[2] = 0;
+ * constr3: 0.1 <= p[1] <= 2.9;
+ * constr4: 0.7 <= p[2];
+ *
+ */
+void mods235(double *p, double *x, int m, int n, void *data)
+{
+  x[0]=0.1*(p[0]-1.0);
+  x[1]=p[1]-p[0]*p[0];
+}
+
+void jacmods235(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=0.1;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=-2.0*p[0];
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+}
+
+/* Boggs and Tolle modified problem 7 (box/linearly constrained), minimum at (0.7, 0.49, 0.19, 1.19, -0.2)
+ * We keep the original objective function & starting point and use the following constraints:
+ *
+ * subject to cons1:
+ *  x[1]+x[2] - x[3] = 1.0;
+ * subject to cons2:
+ *   x[2] - x[4] + x[1] = 0.0;
+ * subject to cons3:
+ *   x[5] + x[1] = 0.5;
+ * subject to cons4:
+ *   x[5]>=-0.3;
+ * subject to cons5:
+ *    x[1]<=0.7;
+ *
+ */
+void modbt7(double *p, double *x, int m, int n, void *data)
+{
+register int i;
+
+  for(i=0; i<n; ++i)
+    x[i]=100.0*(p[1]-p[0]*p[0])*(p[1]-p[0]*p[0]) + (p[0]-1.0)*(p[0]-1.0);
+}
+
+void jacmodbt7(double *p, double *jac, int m, int n, void *data)
+{
+register int i, j;
+
+  for(i=j=0; i<m; ++i){
+    jac[j++]=-400.0*(p[1]-p[0]*p[0])*p[0] + 2.0*p[0] - 2.0;
+    jac[j++]=200.0*(p[1]-p[0]*p[0]);
+    jac[j++]=0.0;
+    jac[j++]=0.0;
+    jac[j++]=0.0;
+  }
+}
+
+/* Equilibrium combustion problem, constrained nonlinear equation from the book by Floudas et al.
+ * Minimum at (0.0034, 31.3265, 0.0684, 0.8595, 0.0370)
+ * constri: p[i]>=0.0001; (i=1..5)
+ * constri+5: p[i]<=100.0; (i=1..5)
+ */
+void combust(double *p, double *x, int m, int n, void *data)
+{
+  double R, R5, R6, R7, R8, R9, R10;
+
+  R=10;
+  R5=0.193;
+  R6=4.10622*1e-4;
+  R7=5.45177*1e-4;
+  R8=4.4975*1e-7;
+  R9=3.40735*1e-5;
+  R10=9.615*1e-7;
+
+  x[0]=p[0]*p[1]+p[0]-3*p[4];
+  x[1]=2*p[0]*p[1]+p[0]+3*R10*p[1]*p[1]+p[1]*p[2]*p[2]+R7*p[1]*p[2]+R9*p[1]*p[3]+R8*p[1]-R*p[4];
+  x[2]=2*p[1]*p[2]*p[2]+R7*p[1]*p[2]+2*R5*p[2]*p[2]+R6*p[2]-8*p[4];
+  x[3]=R9*p[1]*p[3]+2*p[3]*p[3]-4*R*p[4];
+  x[4]=p[0]*p[1]+p[0]+R10*p[1]*p[1]+p[1]*p[2]*p[2]+R7*p[1]*p[2]+R9*p[1]*p[3]+R8*p[1]+R5*p[2]*p[2]+R6*p[2]+p[3]*p[3]-1.0;
+}
+
+void jaccombust(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+  double R, R5, R6, R7, R8, R9, R10;
+
+  R=10;
+  R5=0.193;
+  R6=4.10622*1e-4;
+  R7=5.45177*1e-4;
+  R8=4.4975*1e-7;
+  R9=3.40735*1e-5;
+  R10=9.615*1e-7;
+
+  for(j=0; j<m*n; ++j) jac[j]=0.0;
+
+  j=0;
+  jac[j]=p[1]+1;
+  jac[j+1]=p[0];
+  jac[j+4]=-3;
+
+  j+=m;
+  jac[j]=2*p[1]+1;
+  jac[j+1]=2*p[0]+6*R10*p[1]+p[2]*p[2]+R7*p[2]+R9*p[3]+R8;
+  jac[j+2]=2*p[1]*p[2]+R7*p[1];
+  jac[j+3]=R9*p[1];
+  jac[j+4]=-R;
+
+  j+=m;
+  jac[j+1]=2*p[2]*p[2]+R7*p[2];
+  jac[j+2]=4*p[1]*p[2]+R7*p[1]+4*R5*p[2]+R6;
+  jac[j+4]=-8;
+
+  j+=m;
+  jac[j+1]=R9*p[3];
+  jac[j+3]=R9*p[1]+4*p[3];
+  jac[j+4]=-4*R;
+
+  j+=m;
+  jac[j]=p[1]+1;
+  jac[j+1]=p[0]+2*R10*p[1]+p[2]*p[2]+R7*p[2]+R9*p[3]+R8;
+  jac[j+2]=2*p[1]*p[2]+R7*p[1]+2*R5*p[2]+R6;
+  jac[j+3]=R9*p[1]+2*p[3];
+}
+
+/* Hock - Schittkowski (modified) problem 76 (linear inequalities & equations constrained), minimum at (0.0, 0.00909091, 0.372727, 0.354545)
+ * The non-squared terms in the objective function have been removed, the rhs of constr2 has been changed to 0.4 (from 4)
+ * and constr3 has been changed to an equation.
+ *
+ * constr1: p[0] + 2*p[1] + p[2] + p[3] <= 5;
+ * constr2: 3*p[0] + p[1] + 2*p[2] - p[3] <= 0.4;
+ * constr3: p[1] + 4*p[2] = 1.5;
+ *
+ */
+void modhs76(double *p, double *x, int m, int n, void *data)
+{
+  x[0]=p[0];
+  x[1]=sqrt(0.5)*p[1];
+  x[2]=p[2];
+  x[3]=sqrt(0.5)*p[3];
+}
+
+void jacmodhs76(double *p, double *jac, int m, int n, void *data)
+{
+register int j=0;
+
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=sqrt(0.5);
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=1.0;
+  jac[j++]=0.0;
+
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=0.0;
+  jac[j++]=sqrt(0.5);
+}
+
+
+
+int main()
+{
+register int i, j;
+int problem, ret;
+double p[5], // 5 is max(2, 3, 5)
+	   x[16]; // 16 is max(2, 3, 5, 6, 16)
+int m, n;
+double opts[LM_OPTS_SZ], info[LM_INFO_SZ];
+char *probname[]={
+    "Rosenbrock function",
+    "modified Rosenbrock problem",
+    "Powell's function",
+    "Wood's function",
+    "Meyer's (reformulated) problem",
+    "Osborne's problem",
+    "helical valley function",
+    "Boggs & Tolle's problem #3",
+    "Hock - Schittkowski problem #28",
+    "Hock - Schittkowski problem #48",
+    "Hock - Schittkowski problem #51",
+    "Hock - Schittkowski problem #01",
+    "Hock - Schittkowski modified problem #21",
+    "hatfldb problem",
+    "hatfldc problem",
+    "equilibrium combustion problem",
+    "Hock - Schittkowski modified #1 problem #52",
+    "Schittkowski modified problem #235",
+    "Boggs & Tolle modified problem #7",
+    "Hock - Schittkowski modified #2 problem #52",
+    "Hock - Schittkowski modified problem #76",
+};
+
+  opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
+  opts[4]= LM_DIFF_DELTA; // relevant only if the Jacobian is approximated using finite differences; specifies forward differencing 
+  //opts[4]=-LM_DIFF_DELTA; // specifies central differencing to approximate Jacobian; more accurate but more expensive to compute!
+
+  /* uncomment the appropriate line below to select a minimization problem */
+  problem=
+		  //0; // Rosenbrock function
+		  //1; // modified Rosenbrock problem
+		  //2; // Powell's function
+      //3; // Wood's function
+		  4; // Meyer's (reformulated) problem
+		  //5; // Osborne's problem
+      //6; // helical valley function
+#ifdef HAVE_LAPACK
+      //7; // Boggs & Tolle's problem 3
+      //8; // Hock - Schittkowski problem 28
+      //9; // Hock - Schittkowski problem 48
+      //10; // Hock - Schittkowski problem 51
+#else // no LAPACK
+#ifdef _MSC_VER
+#pragma message("LAPACK not available, some test problems cannot be used")
+#else
+#warning LAPACK not available, some test problems cannot be used
+#endif // _MSC_VER
+
+#endif /* HAVE_LAPACK */
+      //11; // Hock - Schittkowski problem 01
+      //12; // Hock - Schittkowski modified problem 21
+      //13; // hatfldb problem
+      //14; // hatfldc problem
+      //15; // equilibrium combustion problem
+#ifdef HAVE_LAPACK
+      //16; // Hock - Schittkowski modified #1 problem 52
+      //17; // Schittkowski modified problem 235
+      //18; // Boggs & Tolle modified problem #7
+      //19; // Hock - Schittkowski modified #2 problem 52
+      //20; // Hock - Schittkowski modified problem #76"
+#endif /* HAVE_LAPACK */
+				
+  switch(problem){
+  default: fprintf(stderr, "unknown problem specified (#%d)! Note that some minimization problems require LAPACK.\n", problem);
+           exit(1);
+    break;
+
+  case 0:
+  /* Rosenbrock function */
+    m=2; n=2;
+    p[0]=-1.2; p[1]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    ret=dlevmar_der(ros, jacros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    //ret=dlevmar_dif(ros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL);  // no Jacobian
+  break;
+
+  case 1:
+  /* modified Rosenbrock problem */
+    m=2; n=3;
+    p[0]=-1.2; p[1]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    ret=dlevmar_der(modros, jacmodros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    //ret=dlevmar_dif(modros, p, x, m, n, 1000, opts, info, NULL, NULL, NULL);  // no Jacobian
+  break;
+
+  case 2:
+  /* Powell's function */
+    m=2; n=2;
+    p[0]=3.0; p[1]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    ret=dlevmar_der(powell, jacpowell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    //ret=dlevmar_dif(powell, p, x, m, n, 1000, opts, info, NULL, NULL, NULL);		// no Jacobian
+  break;
+
+  case 3:
+  /* Wood's function */
+    m=4; n=6;
+    p[0]=-3.0; p[1]=-1.0; p[2]=-3.0; p[3]=-1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    ret=dlevmar_dif(wood, p, x, m, n, 1000, opts, info, NULL, NULL, NULL);  // no Jacobian
+  break;
+
+  case 4:
+  /* Meyer's data fitting problem */
+    m=3; n=16;
+    p[0]=8.85; p[1]=4.0; p[2]=2.5;
+    x[0]=34.780;	x[1]=28.610; x[2]=23.650; x[3]=19.630;
+    x[4]=16.370;	x[5]=13.720; x[6]=11.540; x[7]=9.744;
+    x[8]=8.261;	x[9]=7.030; x[10]=6.005; x[11]=5.147;
+    x[12]=4.427;	x[13]=3.820; x[14]=3.307; x[15]=2.872;
+    //ret=dlevmar_der(meyer, jacmeyer, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+
+   { double *work, *covar;
+    work=malloc((LM_DIF_WORKSZ(m, n)+m*m)*sizeof(double));
+    if(!work){
+    	fprintf(stderr, "memory allocation request failed in main()\n");
+      exit(1);
+    }
+    covar=work+LM_DIF_WORKSZ(m, n);
+
+    ret=dlevmar_dif(meyer, p, x, m, n, 1000, opts, info, work, covar, NULL); // no Jacobian, caller allocates work memory, covariance estimated
+
+    printf("Covariance of the fit:\n");
+    for(i=0; i<m; ++i){
+      for(j=0; j<m; ++j)
+        printf("%g ", covar[i*m+j]);
+      printf("\n");
+    }
+    printf("\n");
+
+    free(work);
+   }
+
+/* uncomment the following block to verify Jacobian */
+/*
+   {
+    double err[16];
+    dlevmar_chkjac(meyer, jacmeyer, p, m, n, NULL, err); 
+    for(i=0; i<n; ++i) printf("gradient %d, err %g\n", i, err[i]);
+   }
+*/
+  break;
+
+  case 5:
+  /* Osborne's data fitting problem */
+  {
+    double x33[]={
+      8.44E-1, 9.08E-1, 9.32E-1, 9.36E-1, 9.25E-1, 9.08E-1, 8.81E-1,
+      8.5E-1, 8.18E-1, 7.84E-1, 7.51E-1, 7.18E-1, 6.85E-1, 6.58E-1,
+      6.28E-1, 6.03E-1, 5.8E-1, 5.58E-1, 5.38E-1, 5.22E-1, 5.06E-1,
+      4.9E-1, 4.78E-1, 4.67E-1, 4.57E-1, 4.48E-1, 4.38E-1, 4.31E-1,
+      4.24E-1, 4.2E-1, 4.14E-1, 4.11E-1, 4.06E-1};
+
+    m=5; n=33;
+    p[0]=0.5; p[1]=1.5; p[2]=-1.0; p[3]=1.0E-2; p[4]=2.0E-2;
+
+    ret=dlevmar_der(osborne, jacosborne, p, x33, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    //ret=dlevmar_dif(osborne, p, x33, m, n, 1000, opts, info, NULL, NULL, NULL);  // no Jacobian
+  }
+  break;
+
+  case 6:
+  /* helical valley function */
+    m=3; n=3;
+    p[0]=-1.0; p[1]=0.0; p[2]=0.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    ret=dlevmar_der(helval, jachelval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    //ret=dlevmar_dif(helval, p, x, m, n, 1000, opts, info, NULL, NULL, NULL);  // no Jacobian
+  break;
+
+#ifdef HAVE_LAPACK
+  case 7:
+  /* Boggs-Tolle problem 3 */
+    m=5; n=5;
+    p[0]=2.0; p[1]=2.0; p[2]=2.0;
+    p[3]=2.0; p[4]=2.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0,  0.0, 0.0, 1.0, 1.0, -2.0,  0.0, 1.0, 0.0, 0.0, -1.0},
+             b[3]={0.0, 0.0, 0.0};
+
+    ret=dlevmar_lec_der(bt3, jacbt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian
+    //ret=dlevmar_lec_dif(bt3, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian
+    }
+  break;
+
+  case 8:
+  /* Hock - Schittkowski problem 28 */
+    m=3; n=3;
+    p[0]=-4.0; p[1]=1.0; p[2]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[1*3]={1.0, 2.0, 3.0},
+             b[1]={1.0};
+
+    ret=dlevmar_lec_der(hs28, jachs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian
+    //ret=dlevmar_lec_dif(hs28, p, x, m, n, A, b, 1, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian
+    }
+  break;
+
+  case 9:
+  /* Hock - Schittkowski problem 48 */
+    m=5; n=5;
+    p[0]=3.0; p[1]=5.0; p[2]=-3.0;
+    p[3]=2.0; p[4]=-2.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[2*5]={1.0, 1.0, 1.0, 1.0, 1.0,  0.0, 0.0, 1.0, -2.0, -2.0},
+             b[2]={5.0, -3.0};
+
+    ret=dlevmar_lec_der(hs48, jachs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian
+    //ret=dlevmar_lec_dif(hs48, p, x, m, n, A, b, 2, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian
+    }
+  break;
+
+  case 10:
+  /* Hock - Schittkowski problem 51 */
+    m=5; n=5;
+    p[0]=2.5; p[1]=0.5; p[2]=2.0;
+    p[3]=-1.0; p[4]=0.5;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0,  0.0, 0.0, 1.0, 1.0, -2.0,  0.0, 1.0, 0.0, 0.0, -1.0},
+             b[3]={4.0, 0.0, 0.0};
+
+    ret=dlevmar_lec_der(hs51, jachs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, analytic Jacobian
+    //ret=dlevmar_lec_dif(hs51, p, x, m, n, A, b, 3, 1000, opts, info, NULL, NULL, NULL); // lin. constraints, no Jacobian
+    }
+  break;
+
+#endif /* HAVE_LAPACK */
+
+  case 11:
+  /* Hock - Schittkowski problem 01 */
+    m=2; n=2;
+    p[0]=-2.0; p[1]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    //ret=dlevmar_der(hs01, jachs01, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    {
+      double lb[2], ub[2];
+
+      lb[0]=-DBL_MAX; lb[1]=-1.5;
+      ub[0]=ub[1]=DBL_MAX;
+
+      ret=dlevmar_bc_der(hs01, jachs01, p, x, m, n, lb, ub, NULL, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    }
+    break;
+
+  case 12:
+  /* Hock - Schittkowski (modified) problem 21 */
+    m=2; n=2;
+    p[0]=-1.0; p[1]=-1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+    //ret=dlevmar_der(hs21, jachs21, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    {
+      double lb[2], ub[2];
+
+      lb[0]=2.0; lb[1]=-50.0;
+      ub[0]=50.0; ub[1]=50.0;
+
+      ret=dlevmar_bc_der(hs21, jachs21, p, x, m, n, lb, ub, NULL, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    }
+    break;
+
+  case 13:
+  /* hatfldb problem */
+    m=4; n=4;
+    p[0]=p[1]=p[2]=p[3]=0.1;
+    for(i=0; i<n; i++) x[i]=0.0;
+    //ret=dlevmar_der(hatfldb, jachatfldb, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    {
+      double lb[4], ub[4];
+
+      lb[0]=lb[1]=lb[2]=lb[3]=0.0;
+
+      ub[0]=ub[2]=ub[3]=DBL_MAX;
+      ub[1]=0.8;
+
+      ret=dlevmar_bc_der(hatfldb, jachatfldb, p, x, m, n, lb, ub, NULL, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    }
+    break;
+
+  case 14:
+  /* hatfldc problem */
+    m=4; n=4;
+    p[0]=p[1]=p[2]=p[3]=0.9;
+    for(i=0; i<n; i++) x[i]=0.0;
+    //ret=dlevmar_der(hatfldc, jachatfldc, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    {
+      double lb[4], ub[4];
+
+      lb[0]=lb[1]=lb[2]=lb[3]=0.0;
+
+      ub[0]=ub[1]=ub[2]=ub[3]=10.0;
+
+      ret=dlevmar_bc_der(hatfldc, jachatfldc, p, x, m, n, lb, ub, NULL, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    }
+    break;
+
+  case 15:
+  /* equilibrium combustion problem */
+    m=5; n=5;
+    p[0]=p[1]=p[2]=p[3]=p[4]=0.0001;
+    for(i=0; i<n; i++) x[i]=0.0;
+    //ret=dlevmar_der(combust, jaccombust, p, x, m, n, 1000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    {
+      double lb[5], ub[5];
+
+      lb[0]=lb[1]=lb[2]=lb[3]=lb[4]=0.0001;
+
+      ub[0]=ub[1]=ub[2]=ub[3]=ub[4]=100.0;
+
+      ret=dlevmar_bc_der(combust, jaccombust, p, x, m, n, lb, ub, NULL, 5000, opts, info, NULL, NULL, NULL); // with analytic Jacobian
+    }
+    break;
+
+#ifdef HAVE_LAPACK
+  case 16:
+  /* Hock - Schittkowski modified #1 problem 52 */
+    m=5; n=4;
+    p[0]=2.0; p[1]=2.0; p[2]=2.0;
+    p[3]=2.0; p[4]=2.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[3*5]={1.0, 3.0, 0.0, 0.0, 0.0,  0.0, 0.0, 1.0, 1.0, -2.0,  0.0, 1.0, 0.0, 0.0, -1.0},
+             b[3]={0.0, 0.0, 0.0};
+
+      double lb[5], ub[5];
+
+      double weights[5]={2000.0, 2000.0, 2000.0, 2000.0, 2000.0}; // penalty terms weights
+
+      lb[0]=-0.09; lb[1]=0.0; lb[2]=-DBL_MAX; lb[3]=-0.2; lb[4]=0.0;
+      ub[0]=DBL_MAX; ub[1]=0.3; ub[2]=0.25; ub[3]=0.3; ub[4]=0.3;
+
+      ret=dlevmar_blec_der(mod1hs52, jacmod1hs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian
+      //ret=dlevmar_blec_dif(mod1hs52, p, x, m, n, lb, ub, A, b, 3, weights, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian
+    }
+    break;
+
+  case 17:
+  /* Schittkowski modified problem 235 */
+    m=3; n=2;
+    p[0]=-2.0; p[1]=3.0; p[2]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[2*3]={1.0, 0.0, 1.0,  0.0, 1.0, -4.0},
+             b[2]={-1.0, 0.0};
+
+      double lb[3], ub[3];
+
+      lb[0]=-DBL_MAX; lb[1]=0.1; lb[2]=0.7;
+      ub[0]=DBL_MAX; ub[1]=2.9; ub[2]=DBL_MAX;
+
+      ret=dlevmar_blec_der(mods235, jacmods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian
+      //ret=dlevmar_blec_dif(mods235, p, x, m, n, lb, ub, A, b, 2, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian
+    }
+    break;
+
+  case 18:
+  /* Boggs & Tolle modified problem 7 */
+    m=5; n=5;
+    p[0]=-2.0; p[1]=1.0; p[2]=1.0; p[3]=1.0; p[4]=1.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[3*5]={1.0, 1.0, -1.0, 0.0, 0.0,   1.0, 1.0, 0.0, -1.0, 0.0,   1.0, 0.0, 0.0, 0.0, 1.0},
+             b[3]={1.0, 0.0, 0.5};
+
+      double lb[5], ub[5];
+
+      lb[0]=-DBL_MAX; lb[1]=-DBL_MAX; lb[2]=-DBL_MAX; lb[3]=-DBL_MAX; lb[4]=-0.3;
+      ub[0]=0.7;      ub[1]= DBL_MAX; ub[2]= DBL_MAX; ub[3]= DBL_MAX; ub[4]=DBL_MAX;
+
+      ret=dlevmar_blec_der(modbt7, jacmodbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 1000, opts, info, NULL, NULL, NULL); // box & lin. constraints, analytic Jacobian
+      //ret=dlevmar_blec_dif(modbt7, p, x, m, n, lb, ub, A, b, 3, NULL, 10000, opts, info, NULL, NULL, NULL); // box & lin. constraints, no Jacobian
+    }
+    break;
+
+  case 19:
+  /* Hock - Schittkowski modified #2 problem 52 */
+    m=5; n=5;
+    p[0]=2.0; p[1]=2.0; p[2]=2.0;
+    p[3]=2.0; p[4]=2.0;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double C[3*5]={1.0, 3.0, 0.0, 0.0, 0.0,  0.0, 0.0, 1.0, 1.0, -2.0,  0.0, -1.0, 0.0, 0.0, 1.0},
+             d[3]={-1.0, -2.0, -7.0};
+
+      ret=dlevmar_bleic_der(mod2hs52, jacmod2hs52, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, 3, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, analytic Jacobian
+      //ret=dlevmar_bleic_dif(mod2hs52, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, 3, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, no Jacobian
+    }
+    break;
+
+  case 20:
+  /* Hock - Schittkowski modified problem 76 */
+    m=4; n=4;
+    p[0]=0.5; p[1]=0.5; p[2]=0.5; p[3]=0.5;
+    for(i=0; i<n; i++) x[i]=0.0;
+
+    {
+      double A[1*4]={0.0, 1.0, 4.0, 0.0},
+             b[1]={1.5};
+
+      double C[2*4]={-1.0, -2.0, -1.0, -1.0,   -3.0, -1.0, -2.0, 1.0},
+             d[2]={-5.0, -0.4};
+
+      double lb[4]={0.0, 0.0, 0.0, 0.0};
+
+      ret=dlevmar_bleic_der(modhs76, jacmodhs76, p, x, m, n, lb, NULL, A, b, 1, C, d, 2, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, analytic Jacobian
+      //ret=dlevmar_bleic_dif(modhs76, p, x, m, n, lb, NULL, A, b, 1, C, d, 2, 1000, opts, info, NULL, NULL, NULL); // lin. ineq. constraints, no Jacobian
+      /* variations:
+       * if no lb is used, the minimizer is (-0.1135922 0.1330097 0.3417476 0.07572816)
+       * if the rhs of constr2 is 4.0, the minimizer is (0.0, 0.166667, 0.333333, 0.0)
+       */
+    }
+    break;
+
+#endif /* HAVE_LAPACK */
+  } /* switch */
+  
+  printf("Results for %s:\n", probname[problem]);
+  printf("Levenberg-Marquardt returned %d in %g iter, reason %g\nSolution: ", ret, info[5], info[6]);
+  for(i=0; i<m; ++i)
+    printf("%.7g ", p[i]);
+  printf("\n\nMinimization info:\n");
+  for(i=0; i<LM_INFO_SZ; ++i)
+    printf("%g ", info[i]);
+  printf("\n");
+
+  return 0;
+}
Index: ThirdParty/levmar/src/lmlec.c
===================================================================
--- ThirdParty/levmar/src/lmlec.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmlec.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,80 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/*******************************************************************************
+ * Wrappers for linearly constrained Levenberg-Marquardt minimization. The same
+ * core code is used with appropriate #defines to derive single and double
+ * precision versions, see also lmlec_core.c
+ *******************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "levmar.h"
+#include "misc.h"
+
+
+#ifndef HAVE_LAPACK
+
+#ifdef _MSC_VER
+#pragma message("Linearly constrained optimization requires LAPACK and was not compiled!")
+#else
+#warning Linearly constrained optimization requires LAPACK and was not compiled!
+#endif // _MSC_VER
+
+#else // LAPACK present
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "lmlec_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_CNST(x) (x)
+
+#include "lmlec_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
+
+#endif /* HAVE_LAPACK */
+
Index: ThirdParty/levmar/src/lmlec_core.c
===================================================================
--- ThirdParty/levmar/src/lmlec_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/lmlec_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,656 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LM_REAL // not included by lmlec.c
+#error This file should not be compiled directly!
+#endif
+
+
+/* precision-specific definitions */
+#define LMLEC_DATA LM_ADD_PREFIX(lmlec_data)
+#define LMLEC_ELIM LM_ADD_PREFIX(lmlec_elim)
+#define LMLEC_FUNC LM_ADD_PREFIX(lmlec_func)
+#define LMLEC_JACF LM_ADD_PREFIX(lmlec_jacf)
+#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
+#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
+#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
+#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)
+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
+
+#define GEQP3 LM_MK_LAPACK_NAME(geqp3)
+#define ORGQR LM_MK_LAPACK_NAME(orgqr)
+#define TRTRI LM_MK_LAPACK_NAME(trtri)
+
+struct LMLEC_DATA{
+  LM_REAL *c, *Z, *p, *jac;
+  int ncnstr;
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
+  void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
+  void *adata;
+};
+
+/* prototypes for LAPACK routines */
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int GEQP3(int *m, int *n, LM_REAL *a, int *lda, int *jpvt,
+                   LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
+
+extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau,
+                   LM_REAL *work, int *lwork, int *info);
+
+extern int TRTRI(char *uplo, char *diag, int *n, LM_REAL *a, int *lda, int *info);
+#ifdef __cplusplus
+}
+#endif
+
+/*
+ * This function implements an elimination strategy for linearly constrained
+ * optimization problems. The strategy relies on QR decomposition to transform
+ * an optimization problem constrained by Ax=b to an equivalent, unconstrained
+ * one. Also referred to as "null space" or "reduced Hessian" method.
+ * See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright
+ * for details.
+ *
+ * A is mxn with m<=n and rank(A)=m
+ * Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that
+ * their columns are orthonormal and every x can be written as x=Y*b + Z*x_z=
+ * c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an
+ * arbitrary vector of dimension n-m. Then, the problem of minimizing f(x)
+ * subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints.
+ * The computed Y and Z are such that any solution of Ax=b can be written as
+ * x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0
+ * and Z spans the null space of A.
+ *
+ * The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not
+ * computed. Also, Y can be NULL in which case it is not referenced.
+ * The function returns LM_ERROR in case of error, A's computed rank if successful
+ *
+ */
+static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n)
+{
+static LM_REAL eps=LM_CNST(-1.0);
+
+LM_REAL *buf=NULL;
+LM_REAL *a, *tau, *work, *r, aux;
+register LM_REAL tmp;
+int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;
+int info, rank, *jpvt, tot_sz, mintmn, tm, tn;
+register int i, j, k;
+
+  if(m>n){
+    fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");
+    return LM_ERROR;
+  }
+
+  tm=n; tn=m; // transpose dimensions
+  mintmn=m;
+
+  /* calculate required memory size */
+  worksz=-1; // workspace query. Optimal work size is returned in aux
+  //ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, NULL, (int *)&tm, NULL, (LM_REAL *)&aux, &worksz, &info);
+  GEQP3((int *)&tm, (int *)&tn, NULL, (int *)&tm, NULL, NULL, (LM_REAL *)&aux, (int *)&worksz, &info);
+  worksz=(int)aux;
+  a_sz=tm*tm; // tm*tn is enough for xgeqp3()
+  jpvt_sz=tn;
+  tau_sz=mintmn;
+  r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank
+  Y_sz=(Y)? 0 : tm*tn;
+
+  tot_sz=(a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL) + jpvt_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+  buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */
+  if(!buf){
+    fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");
+    return LM_ERROR;
+  }
+
+  a=buf;
+  tau=a+a_sz;
+  r=tau+tau_sz;
+  work=r+r_sz;
+  if(!Y){
+    Y=work+worksz;
+    jpvt=(int *)(Y+Y_sz);
+  }
+  else
+    jpvt=(int *)(work+worksz);
+
+  /* copy input array so that LAPACK won't destroy it. Note that copying is
+   * done in row-major order, which equals A^T in column-major
+   */
+  for(i=0; i<tm*tn; ++i)
+      a[i]=A[i];
+
+  /* clear jpvt */
+  for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;
+
+  /* rank revealing QR decomposition of A^T*/
+  GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);
+  //dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);
+  /* error checking */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);
+    }
+    else if(info>0){
+      fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);
+    }
+    free(buf);
+    return LM_ERROR;
+  }
+  /* the upper triangular part of a now contains the upper triangle of the unpermuted R */
+
+  if(eps<0.0){
+    LM_REAL aux;
+
+    /* compute machine epsilon. DBL_EPSILON should do also */
+    for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
+                              ;
+    eps*=LM_CNST(2.0);
+  }
+
+  tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)
+  tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small
+  /* compute A^T's numerical rank by counting the non-zeros in R's diagonal */
+  for(i=rank=0; i<mintmn; ++i)
+    if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */
+    else break; /* diagonal is arranged in absolute decreasing order */
+
+  if(rank<tn){
+    fprintf(stderr, RCAT("\nConstraints matrix in ",  LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"
+            "Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);
+    free(buf);
+    return LM_ERROR;
+  }
+
+  /* compute the permuted inverse transpose of R */
+  /* first, copy R from the upper triangular part of a to the lower part of r (thus transposing it). R is rank x rank */
+  for(j=0; j<rank; ++j){
+    for(i=0; i<=j; ++i)
+      r[j+i*rank]=a[i+j*tm];
+    for(i=j+1; i<rank; ++i)
+      r[j+i*rank]=0.0; // upper part is zero
+  }
+  /* r now contains R^T */
+
+  /* compute the inverse */
+  TRTRI("L", "N", (int *)&rank, r, (int *)&rank, &info);
+  /* error checking */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);
+    }
+    else if(info>0){
+      fprintf(stderr, RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);
+    }
+    free(buf);
+    return LM_ERROR;
+  }
+
+  /* finally, permute R^-T using Y as intermediate storage */
+  for(j=0; j<rank; ++j)
+    for(i=0, k=jpvt[j]-1; i<rank; ++i)
+      Y[i+k*rank]=r[i+j*rank];
+
+  for(i=0; i<rank*rank; ++i) // copy back to r
+    r[i]=Y[i];
+
+  /* resize a to be tm x tm, filling with zeroes */
+  for(i=tm*tn; i<tm*tm; ++i)
+    a[i]=0.0;
+
+  /* compute Q in a as the product of elementary reflectors. Q is tm x tm */
+  ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);
+  /* error checking */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);
+    }
+    else if(info>0){
+      fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);
+    }
+    free(buf);
+    return LM_ERROR;
+  }
+
+  /* compute Y=Q_1*R^-T*P^T. Y is tm x rank */
+  for(i=0; i<tm; ++i)
+    for(j=0; j<rank; ++j){
+      for(k=0, tmp=0.0; k<rank; ++k)
+        tmp+=a[i+k*tm]*r[k+j*rank];
+      Y[i*rank+j]=tmp;
+    }
+
+  if(b && c){
+    /* compute c=Y*b */
+    for(i=0; i<tm; ++i){
+      for(j=0, tmp=0.0; j<rank; ++j)
+        tmp+=Y[i*rank+j]*b[j];
+
+      c[i]=tmp;
+    }
+  }
+
+  /* copy Q_2 into Z. Z is tm x (tm-rank) */
+  for(j=0; j<tm-rank; ++j)
+    for(i=0, k=j+rank; i<tm; ++i)
+      Z[i*(tm-rank)+j]=a[i+k*tm];
+
+  free(buf);
+
+  return rank;
+}
+
+/* constrained measurements: given pp, compute the measurements at c + Z*pp */
+static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)
+{
+struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
+int m;
+register int i, j;
+register LM_REAL sum;
+LM_REAL *c, *Z, *p, *Zimm;
+
+  m=mm+data->ncnstr;
+  c=data->c;
+  Z=data->Z;
+  p=data->p;
+  /* p=c + Z*pp */
+  for(i=0; i<m; ++i){
+    Zimm=Z+i*mm;
+    for(j=0, sum=c[i]; j<mm; ++j)
+      sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
+    p[i]=sum;
+  }
+
+  (*(data->func))(p, hx, m, n, data->adata);
+}
+
+/* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp
+ * Using the chain rule, the Jacobian with respect to pp equals the
+ * product of the Jacobian with respect to p (at c + Z*pp) times Z
+ */
+static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)
+{
+struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
+int m;
+register int i, j, l;
+register LM_REAL sum, *aux1, *aux2;
+LM_REAL *c, *Z, *p, *jac; 
+
+  m=mm+data->ncnstr;
+  c=data->c;
+  Z=data->Z;
+  p=data->p;
+  jac=data->jac;
+  /* p=c + Z*pp */
+  for(i=0; i<m; ++i){
+    aux1=Z+i*mm;
+    for(j=0, sum=c[i]; j<mm; ++j)
+      sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
+    p[i]=sum;
+  }
+
+  (*(data->jacf))(p, jac, m, n, data->adata);
+
+  /* compute jac*Z in jacjac */
+  if(n*m<=__BLOCKSZ__SQ){ // this is a small problem
+    /* This is the straightforward way to compute jac*Z. However, due to
+     * its noncontinuous memory access pattern, it incures many cache misses when
+     * applied to large minimization problems (i.e. problems involving a large
+     * number of free variables and measurements), in which jac is too large to
+     * fit in the L1 cache. For such problems, a cache-efficient blocking scheme
+     * is preferable. On the other hand, the straightforward algorithm is faster
+     * on small problems since in this case it avoids the overheads of blocking.
+     */
+
+    for(i=0; i<n; ++i){
+      aux1=jac+i*m;
+      aux2=jacjac+i*mm;
+      for(j=0; j<mm; ++j){
+        for(l=0, sum=0.0; l<m; ++l)
+          sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];
+
+        aux2[j]=sum; // jacjac[i*mm+j]=sum;
+      }
+    }
+  }
+  else{ // this is a large problem
+    /* Cache efficient computation of jac*Z based on blocking
+     */
+#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
+    register int jj, ll;
+
+    for(jj=0; jj<mm; jj+=__BLOCKSZ__){
+      for(i=0; i<n; ++i){
+        aux1=jacjac+i*mm;
+        for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)
+          aux1[j]=0.0; //jacjac[i*mm+j]=0.0;
+      }
+
+      for(ll=0; ll<m; ll+=__BLOCKSZ__){
+        for(i=0; i<n; ++i){
+          aux1=jacjac+i*mm; aux2=jac+i*m;
+          for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){
+            sum=0.0;
+            for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)
+              sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];
+            aux1[j]+=sum; //jacjac[i*mm+j]+=sum;
+          }
+        }
+      }
+    }
+  }
+}
+#undef __MIN__
+
+
+/* 
+ * This function is similar to LEVMAR_DER except that the minimization
+ * is performed subject to the linear constraints A p=b, A is kxm, b kx1
+ *
+ * This function requires an analytic Jacobian. In case the latter is unavailable,
+ * use LEVMAR_LEC_DIF() bellow
+ *
+ */
+int LEVMAR_LEC_DER(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */ 
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *A,         /* I: constraints matrix, kxm */
+  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
+  int k,              /* I: number of constraints (i.e. A's #rows) */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
+                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_LEC_DER_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMLEC_DATA data;
+  LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
+  int mm, ret;
+  register int i, j;
+  register LM_REAL tmp;
+  LM_REAL locinfo[LM_INFO_SZ];
+
+  if(!jacf){
+    fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_LEC_DER)
+      RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");
+    return LM_ERROR;
+  }
+
+  mm=m-k;
+
+  if(n<mm){
+    fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
+    return LM_ERROR;
+  }
+
+  ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));
+  if(!ptr){
+    fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));
+    return LM_ERROR;
+  }
+  data.p=p;
+  p0=ptr;
+  data.c=p0+m;
+  data.Z=Z=data.c+m;
+  data.jac=data.Z+m*mm;
+  pp=data.jac+n*m;
+  data.ncnstr=k;
+  data.func=func;
+  data.jacf=jacf;
+  data.adata=adata;
+
+  ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
+  if(ret==LM_ERROR){
+    free(ptr);
+    return LM_ERROR;
+  }
+
+  /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
+   * Due to orthogonality, Z^T Z = I and the last equation
+   * becomes pp=Z^T*(p-c). Also, save the starting p in p0
+   */
+  for(i=0; i<m; ++i){
+    p0[i]=p[i];
+    p[i]-=data.c[i];
+  }
+
+  /* Z^T*(p-c) */
+  for(i=0; i<mm; ++i){
+    for(j=0, tmp=0.0; j<m; ++j)
+      tmp+=Z[j*mm+i]*p[j];
+    pp[i]=tmp;
+  }
+
+  /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
+  for(i=0; i<m; ++i){
+    Zimm=Z+i*mm;
+    for(j=0, tmp=data.c[i]; j<mm; ++j)
+      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
+    if(FABS(tmp-p0[i])>LM_CNST(1E-03))
+      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",
+                      i, p0[i], tmp);
+  }
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */
+  /* note that covariance computation is not requested from LEVMAR_DER() */
+  ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
+
+  /* p=c + Z*pp */
+  for(i=0; i<m; ++i){
+    Zimm=Z+i*mm;
+    for(j=0, tmp=data.c[i]; j<mm; ++j)
+      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
+    p[i]=tmp;
+  }
+
+  /* compute the covariance from the Jacobian in data.jac */
+  if(covar){
+    LEVMAR_TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */
+    LEVMAR_COVAR(covar, covar, info[1], m, n);
+  }
+
+  free(ptr);
+
+  return ret;
+}
+
+/* Similar to the LEVMAR_LEC_DER() function above, except that the Jacobian is approximated
+ * with the aid of finite differences (forward or central, see the comment for the opts argument)
+ */
+int LEVMAR_LEC_DIF(
+  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
+  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
+  LM_REAL *x,         /* I: measurement vector. NULL implies a zero vector */
+  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
+  int n,              /* I: measurement vector dimension */
+  LM_REAL *A,         /* I: constraints matrix, kxm */
+  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
+  int k,              /* I: number of constraints (i.e. A's #rows) */
+  int itmax,          /* I: maximum number of iterations */
+  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
+                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
+                       * the step used in difference approximation to the Jacobian. Set to NULL for defaults to be used.
+                       * If \delta<0, the Jacobian is approximated with central differences which are more accurate
+                       * (but slower!) compared to the forward differences employed by default. 
+                       */
+  LM_REAL info[LM_INFO_SZ],
+					           /* O: information regarding the minimization. Set to NULL if don't care
+                      * info[0]= ||e||_2 at initial p.
+                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
+                      * info[5]= # iterations,
+                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
+                      *                                 2 - stopped by small Dp
+                      *                                 3 - stopped by itmax
+                      *                                 4 - singular matrix. Restart from current p with increased mu 
+                      *                                 5 - no further error reduction is possible. Restart with increased mu
+                      *                                 6 - stopped by small ||e||_2
+                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
+                      * info[7]= # function evaluations
+                      * info[8]= # Jacobian evaluations
+                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
+                      */
+  LM_REAL *work,     /* working memory at least LM_LEC_DIF_WORKSZ() reals large, allocated if NULL */
+  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
+  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
+                      * Set to NULL if not needed
+                      */
+{
+  struct LMLEC_DATA data;
+  LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
+  int mm, ret;
+  register int i, j;
+  register LM_REAL tmp;
+  LM_REAL locinfo[LM_INFO_SZ];
+
+  mm=m-k;
+
+  if(n<mm){
+    fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
+    return LM_ERROR;
+  }
+
+  ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));
+  if(!ptr){
+    fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
+    return LM_ERROR;
+  }
+  data.p=p;
+  p0=ptr;
+  data.c=p0+m;
+  data.Z=Z=data.c+m;
+  data.jac=NULL;
+  pp=data.Z+m*mm;
+  data.ncnstr=k;
+  data.func=func;
+  data.jacf=NULL;
+  data.adata=adata;
+
+  ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
+  if(ret==LM_ERROR){
+    free(ptr);
+    return LM_ERROR;
+  }
+
+  /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
+   * Due to orthogonality, Z^T Z = I and the last equation
+   * becomes pp=Z^T*(p-c). Also, save the starting p in p0
+   */
+  for(i=0; i<m; ++i){
+    p0[i]=p[i];
+    p[i]-=data.c[i];
+  }
+
+  /* Z^T*(p-c) */
+  for(i=0; i<mm; ++i){
+    for(j=0, tmp=0.0; j<m; ++j)
+      tmp+=Z[j*mm+i]*p[j];
+    pp[i]=tmp;
+  }
+
+  /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
+  for(i=0; i<m; ++i){
+    Zimm=Z+i*mm;
+    for(j=0, tmp=data.c[i]; j<mm; ++j)
+      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
+    if(FABS(tmp-p0[i])>LM_CNST(1E-03))
+      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",
+                      i, p0[i], tmp);
+  }
+
+  if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */
+  /* note that covariance computation is not requested from LEVMAR_DIF() */
+  ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);
+
+  /* p=c + Z*pp */
+  for(i=0; i<m; ++i){
+    Zimm=Z+i*mm;
+    for(j=0, tmp=data.c[i]; j<mm; ++j)
+      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
+    p[i]=tmp;
+  }
+
+  /* compute the Jacobian with finite differences and use it to estimate the covariance */
+  if(covar){
+    LM_REAL *hx, *wrk, *jac;
+
+    hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));
+    if(!hx){
+      fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
+      free(ptr);
+      return LM_ERROR;
+    }
+
+    wrk=hx+n;
+    jac=wrk+n;
+
+    (*func)(p, hx, m, n, adata); /* evaluate function at p */
+    LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */
+    LEVMAR_TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */
+    LEVMAR_COVAR(covar, covar, info[1], m, n);
+    free(hx);
+  }
+
+  free(ptr);
+
+  return ret;
+}
+
+/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef LMLEC_DATA
+#undef LMLEC_ELIM
+#undef LMLEC_FUNC
+#undef LMLEC_JACF
+#undef LEVMAR_FDIF_FORW_JAC_APPROX
+#undef LEVMAR_COVAR
+#undef LEVMAR_TRANS_MAT_MAT_MULT
+#undef LEVMAR_LEC_DER
+#undef LEVMAR_LEC_DIF
+#undef LEVMAR_DER
+#undef LEVMAR_DIF
+
+#undef GEQP3
+#undef ORGQR
+#undef TRTRI
Index: ThirdParty/levmar/src/misc.c
===================================================================
--- ThirdParty/levmar/src/misc.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/misc.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,70 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+/******************************************************************************** 
+ * Miscelaneous functions for Levenberg-Marquardt nonlinear minimization. The
+ * same core code is used with appropriate #defines to derive single and double
+ * precision versions, see also misc_core.c
+ ********************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+
+#include "levmar.h"
+#include "misc.h"
+
+#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
+#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
+#endif
+
+#ifdef LM_SNGL_PREC
+/* single precision (float) definitions */
+#define LM_REAL float
+#define LM_PREFIX s
+
+#define LM_REAL_EPSILON FLT_EPSILON
+#define __SUBCNST(x) x##F
+#define LM_CNST(x) __SUBCNST(x) // force substitution
+
+#include "misc_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_EPSILON
+#undef __SUBCNST
+#undef LM_CNST
+#endif /* LM_SNGL_PREC */
+
+#ifdef LM_DBL_PREC
+/* double precision definitions */
+#define LM_REAL double
+#define LM_PREFIX d
+
+#define LM_REAL_EPSILON DBL_EPSILON
+#define LM_CNST(x) (x)
+
+#include "misc_core.c" // read in core code
+
+#undef LM_REAL
+#undef LM_PREFIX
+#undef LM_REAL_EPSILON
+#undef LM_CNST
+#endif /* LM_DBL_PREC */
Index: ThirdParty/levmar/src/misc.h
===================================================================
--- ThirdParty/levmar/src/misc.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/misc.h	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,114 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef _MISC_H_
+#define _MISC_H_
+
+/* common suffix for LAPACK subroutines. Define empty in case of no prefix. */
+#define LM_LAPACK_SUFFIX _
+//#define LM_LAPACK_SUFFIX  // define empty
+
+/* common prefix for BLAS subroutines. Leave undefined in case of no prefix.
+ * You might also need to modify LM_BLAS_PREFIX below
+ */
+/* f2c'd BLAS */
+//#define LM_BLAS_PREFIX f2c_
+/* C BLAS */
+//#define LM_BLAS_PREFIX cblas_
+
+/* common suffix for BLAS subroutines */
+//#define LM_BLAS_SUFFIX  // define empty if a f2c_ or cblas_ prefix was defined for LM_BLAS_PREFIX above
+#define LM_BLAS_SUFFIX _ // use this in case of no BLAS prefix
+
+
+#define LCAT_(a, b)    #a b
+#define LCAT(a, b)    LCAT_(a, b) // force substitution
+#define RCAT_(a, b)    a #b
+#define RCAT(a, b)    RCAT_(a, b) // force substitution
+
+#define LM_MK_LAPACK_NAME(s)  LM_ADD_PREFIX(LM_CAT_(s, LM_LAPACK_SUFFIX))
+
+#ifdef LM_BLAS_PREFIX
+#define LM_MK_BLAS_NAME(s) LM_CAT_(LM_BLAS_PREFIX, LM_ADD_PREFIX(LM_CAT_(s, LM_BLAS_SUFFIX)))
+#else
+#define LM_MK_BLAS_NAME(s) LM_ADD_PREFIX(LM_CAT_(s, LM_BLAS_SUFFIX))
+#endif
+
+
+#define __BLOCKSZ__       32 /* block size for cache-friendly matrix-matrix multiply. It should be
+                              * such that __BLOCKSZ__^2*sizeof(LM_REAL) is smaller than the CPU (L1)
+                              * data cache size. Notice that a value of 32 when LM_REAL=double assumes
+                              * an 8Kb L1 data cache (32*32*8=8K). This is a concervative choice since
+                              * newer Pentium 4s have a L1 data cache of size 16K, capable of holding
+                              * up to 45x45 double blocks.
+                              */
+#define __BLOCKSZ__SQ    (__BLOCKSZ__)*(__BLOCKSZ__)
+
+/* add a prefix in front of a token */
+#define LM_CAT__(a, b) a ## b
+#define LM_CAT_(a, b) LM_CAT__(a, b) // force substitution
+#define LM_ADD_PREFIX(s) LM_CAT_(LM_PREFIX, s)
+
+#define FABS(x) (((x)>=0.0)? (x) : -(x))
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* blocking-based matrix multiply */
+extern void slevmar_trans_mat_mat_mult(float *a, float *b, int n, int m);
+extern void dlevmar_trans_mat_mat_mult(double *a, double *b, int n, int m);
+
+/* forward finite differences */
+extern void slevmar_fdif_forw_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),
+					float *p, float *hx, float *hxx, float delta,
+					float *jac, int m, int n, void *adata);
+extern void dlevmar_fdif_forw_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),
+					double *p, double *hx, double *hxx, double delta,
+					double *jac, int m, int n, void *adata);
+
+/* central finite differences */
+extern void slevmar_fdif_cent_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),
+          float *p, float *hxm, float *hxp, float delta,
+          float *jac, int m, int n, void *adata);
+extern void dlevmar_fdif_cent_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),
+          double *p, double *hxm, double *hxp, double delta,
+          double *jac, int m, int n, void *adata);
+
+/* e=x-y and ||e|| */
+extern float  slevmar_L2nrmxmy(float *e, float *x, float *y, int n);
+extern double dlevmar_L2nrmxmy(double *e, double *x, double *y, int n);
+
+/* covariance of LS fit */
+extern int slevmar_covar(float *JtJ, float *C, float sumsq, int m, int n);
+extern int dlevmar_covar(double *JtJ, double *C, double sumsq, int m, int n);
+
+/* box constraints consistency check */
+extern int slevmar_box_check(float *lb, float *ub, int m);
+extern int dlevmar_box_check(double *lb, double *ub, int m);
+
+/* Cholesky */
+extern int slevmar_chol(float *C, float *W, int m);
+extern int dlevmar_chol(double *C, double *W, int m);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MISC_H_ */
Index: ThirdParty/levmar/src/misc_core.c
===================================================================
--- ThirdParty/levmar/src/misc_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
+++ ThirdParty/levmar/src/misc_core.c	(revision 8ce1a95e94bdc842511db41cb292238a19558df0)
@@ -0,0 +1,826 @@
+/////////////////////////////////////////////////////////////////////////////////
+// 
+//  Levenberg - Marquardt non-linear minimization algorithm
+//  Copyright (C) 2004-05  Manolis Lourakis (lourakis at ics forth gr)
+//  Institute of Computer Science, Foundation for Research & Technology - Hellas
+//  Heraklion, Crete, Greece.
+//
+//  This program is free software; you can redistribute it and/or modify
+//  it under the terms of the GNU General Public License as published by
+//  the Free Software Foundation; either version 2 of the License, or
+//  (at your option) any later version.
+//
+//  This program is distributed in the hope that it will be useful,
+//  but WITHOUT ANY WARRANTY; without even the implied warranty of
+//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//  GNU General Public License for more details.
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LM_REAL // not included by misc.c
+#error This file should not be compiled directly!
+#endif
+
+
+/* precision-specific definitions */
+#define LEVMAR_CHKJAC LM_ADD_PREFIX(levmar_chkjac)
+#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
+#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
+#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
+#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
+#define LEVMAR_STDDEV LM_ADD_PREFIX(levmar_stddev)
+#define LEVMAR_CORCOEF LM_ADD_PREFIX(levmar_corcoef)
+#define LEVMAR_R2 LM_ADD_PREFIX(levmar_R2)
+#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
+#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
+
+#ifdef HAVE_LAPACK
+#define LEVMAR_PSEUDOINVERSE LM_ADD_PREFIX(levmar_pseudoinverse)
+static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/* BLAS matrix multiplication, LAPACK SVD & Cholesky routines */
+#define GEMM LM_MK_BLAS_NAME(gemm)
+/* C := alpha*op( A )*op( B ) + beta*C */
+extern void GEMM(char *transa, char *transb, int *m, int *n, int *k,
+          LM_REAL *alpha, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, LM_REAL *beta, LM_REAL *c, int *ldc);
+
+#define GESVD LM_MK_LAPACK_NAME(gesvd)
+#define GESDD LM_MK_LAPACK_NAME(gesdd)
+extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,
+                 LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);
+
+/* lapack 3.0 new SVD routine, faster than xgesvd() */
+extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,
+                 LM_REAL *work, int *lwork, int *iwork, int *info);
+
+/* Cholesky decomposition */
+#define POTF2 LM_MK_LAPACK_NAME(potf2)
+extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);
+#ifdef __cplusplus
+}
+#endif
+
+#define LEVMAR_CHOLESKY LM_ADD_PREFIX(levmar_chol)
+
+#else /* !HAVE_LAPACK */
+#define LEVMAR_LUINVERSE LM_ADD_PREFIX(levmar_LUinverse_noLapack)
+
+static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m);
+#endif /* HAVE_LAPACK */
+
+/* blocked multiplication of the transpose of the nxm matrix a with itself (i.e. a^T a)
+ * using a block size of bsize. The product is returned in b.
+ * Since a^T a is symmetric, its computation can be sped up by computing only its
+ * upper triangular part and copying it to the lower part.
+ *
+ * More details on blocking can be found at 
+ * http://www-2.cs.cmu.edu/afs/cs/academic/class/15213-f02/www/R07/section_a/Recitation07-SectionA.pdf
+ */
+void LEVMAR_TRANS_MAT_MAT_MULT(LM_REAL *a, LM_REAL *b, int n, int m)
+{
+#ifdef HAVE_LAPACK /* use BLAS matrix multiply */
+
+LM_REAL alpha=LM_CNST(1.0), beta=LM_CNST(0.0);
+  /* Fool BLAS to compute a^T*a avoiding transposing a: a is equivalent to a^T in column major,
+   * therefore BLAS computes a*a^T with a and a*a^T in column major, which is equivalent to
+   * computing a^T*a in row major!
+   */
+  GEMM("N", "T", &m, &m, &n, &alpha, a, &m, a, &m, &beta, b, &m);
+
+#else /* no LAPACK, use blocking-based multiply */
+
+register int i, j, k, jj, kk;
+register LM_REAL sum, *bim, *akm;
+const int bsize=__BLOCKSZ__;
+
+#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
+#define __MAX__(x, y) (((x)>=(y))? (x) : (y))
+
+  /* compute upper triangular part using blocking */
+  for(jj=0; jj<m; jj+=bsize){
+    for(i=0; i<m; ++i){
+      bim=b+i*m;
+      for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j)
+        bim[j]=0.0; //b[i*m+j]=0.0;
+    }
+
+    for(kk=0; kk<n; kk+=bsize){
+      for(i=0; i<m; ++i){
+        bim=b+i*m;
+        for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j){
+          sum=0.0;
+          for(k=kk; k<__MIN__(kk+bsize, n); ++k){
+            akm=a+k*m;
+            sum+=akm[i]*akm[j]; //a[k*m+i]*a[k*m+j];
+          }
+          bim[j]+=sum; //b[i*m+j]+=sum;
+        }
+      }
+    }
+  }
+
+  /* copy upper triangular part to the lower one */
+  for(i=0; i<m; ++i)
+    for(j=0; j<i; ++j)
+      b[i*m+j]=b[j*m+i];
+
+#undef __MIN__
+#undef __MAX__
+
+#endif /* HAVE_LAPACK */
+}
+
+/* forward finite difference approximation to the Jacobian of func */
+void LEVMAR_FDIF_FORW_JAC_APPROX(
+    void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+													   /* function to differentiate */
+    LM_REAL *p,              /* I: current parameter estimate, mx1 */
+    LM_REAL *hx,             /* I: func evaluated at p, i.e. hx=func(p), nx1 */
+    LM_REAL *hxx,            /* W/O: work array for evaluating func(p+delta), nx1 */
+    LM_REAL delta,           /* increment for computing the Jacobian */
+    LM_REAL *jac,            /* O: array for storing approximated Jacobian, nxm */
+    int m,
+    int n,
+    void *adata)
+{
+register int i, j;
+LM_REAL tmp;
+register LM_REAL d;
+
+  for(j=0; j<m; ++j){
+    /* determine d=max(1E-04*|p[j]|, delta), see HZ */
+    d=LM_CNST(1E-04)*p[j]; // force evaluation
+    d=FABS(d);
+    if(d<delta)
+      d=delta;
+
+    tmp=p[j];
+    p[j]+=d;
+
+    (*func)(p, hxx, m, n, adata);
+
+    p[j]=tmp; /* restore */
+
+    d=LM_CNST(1.0)/d; /* invert so that divisions can be carried out faster as multiplications */
+    for(i=0; i<n; ++i){
+      jac[i*m+j]=(hxx[i]-hx[i])*d;
+    }
+  }
+}
+
+/* central finite difference approximation to the Jacobian of func */
+void LEVMAR_FDIF_CENT_JAC_APPROX(
+    void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+													   /* function to differentiate */
+    LM_REAL *p,              /* I: current parameter estimate, mx1 */
+    LM_REAL *hxm,            /* W/O: work array for evaluating func(p-delta), nx1 */
+    LM_REAL *hxp,            /* W/O: work array for evaluating func(p+delta), nx1 */
+    LM_REAL delta,           /* increment for computing the Jacobian */
+    LM_REAL *jac,            /* O: array for storing approximated Jacobian, nxm */
+    int m,
+    int n,
+    void *adata)
+{
+register int i, j;
+LM_REAL tmp;
+register LM_REAL d;
+
+  for(j=0; j<m; ++j){
+    /* determine d=max(1E-04*|p[j]|, delta), see HZ */
+    d=LM_CNST(1E-04)*p[j]; // force evaluation
+    d=FABS(d);
+    if(d<delta)
+      d=delta;
+
+    tmp=p[j];
+    p[j]-=d;
+    (*func)(p, hxm, m, n, adata);
+
+    p[j]=tmp+d;
+    (*func)(p, hxp, m, n, adata);
+    p[j]=tmp; /* restore */
+
+    d=LM_CNST(0.5)/d; /* invert so that divisions can be carried out faster as multiplications */
+    for(i=0; i<n; ++i){
+      jac[i*m+j]=(hxp[i]-hxm[i])*d;
+    }
+  }
+}
+
+/* 
+ * Check the Jacobian of a n-valued nonlinear function in m variables
+ * evaluated at a point p, for consistency with the function itself.
+ *
+ * Based on fortran77 subroutine CHKDER by
+ * Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
+ * Argonne National Laboratory. MINPACK project. March 1980.
+ *
+ *
+ * func points to a function from R^m --> R^n: Given a p in R^m it yields hx in R^n
+ * jacf points to a function implementing the Jacobian of func, whose correctness
+ *     is to be tested. Given a p in R^m, jacf computes into the nxm matrix j the
+ *     Jacobian of func at p. Note that row i of j corresponds to the gradient of
+ *     the i-th component of func, evaluated at p.
+ * p is an input array of length m containing the point of evaluation.
+ * m is the number of variables
+ * n is the number of functions
+ * adata points to possible additional data and is passed uninterpreted
+ *     to func, jacf.
+ * err is an array of length n. On output, err contains measures
+ *     of correctness of the respective gradients. if there is
+ *     no severe loss of significance, then if err[i] is 1.0 the
+ *     i-th gradient is correct, while if err[i] is 0.0 the i-th
+ *     gradient is incorrect. For values of err between 0.0 and 1.0,
+ *     the categorization is less certain. In general, a value of
+ *     err[i] greater than 0.5 indicates that the i-th gradient is
+ *     probably correct, while a value of err[i] less than 0.5
+ *     indicates that the i-th gradient is probably incorrect.
+ *
+ *
+ * The function does not perform reliably if cancellation or
+ * rounding errors cause a severe loss of significance in the
+ * evaluation of a function. therefore, none of the components
+ * of p should be unusually small (in particular, zero) or any
+ * other value which may cause loss of significance.
+ */
+
+void LEVMAR_CHKJAC(
+    void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+    void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
+    LM_REAL *p, int m, int n, void *adata, LM_REAL *err)
+{
+LM_REAL factor=LM_CNST(100.0);
+LM_REAL one=LM_CNST(1.0);
+LM_REAL zero=LM_CNST(0.0);
+LM_REAL *fvec, *fjac, *pp, *fvecp, *buf;
+
+register int i, j;
+LM_REAL eps, epsf, temp, epsmch;
+LM_REAL epslog;
+int fvec_sz=n, fjac_sz=n*m, pp_sz=m, fvecp_sz=n;
+
+  epsmch=LM_REAL_EPSILON;
+  eps=(LM_REAL)sqrt(epsmch);
+
+  buf=(LM_REAL *)malloc((fvec_sz + fjac_sz + pp_sz + fvecp_sz)*sizeof(LM_REAL));
+  if(!buf){
+    fprintf(stderr, LCAT(LEVMAR_CHKJAC, "(): memory allocation request failed\n"));
+    exit(1);
+  }
+  fvec=buf;
+  fjac=fvec+fvec_sz;
+  pp=fjac+fjac_sz;
+  fvecp=pp+pp_sz;
+
+  /* compute fvec=func(p) */
+  (*func)(p, fvec, m, n, adata);
+
+  /* compute the Jacobian at p */
+  (*jacf)(p, fjac, m, n, adata);
+
+  /* compute pp */
+  for(j=0; j<m; ++j){
+    temp=eps*FABS(p[j]);
+    if(temp==zero) temp=eps;
+    pp[j]=p[j]+temp;
+  }
+
+  /* compute fvecp=func(pp) */
+  (*func)(pp, fvecp, m, n, adata);
+
+  epsf=factor*epsmch;
+  epslog=(LM_REAL)log10(eps);
+
+  for(i=0; i<n; ++i)
+    err[i]=zero;
+
+  for(j=0; j<m; ++j){
+    temp=FABS(p[j]);
+    if(temp==zero) temp=one;
+
+    for(i=0; i<n; ++i)
+      err[i]+=temp*fjac[i*m+j];
+  }
+
+  for(i=0; i<n; ++i){
+    temp=one;
+    if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
+        temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
+    err[i]=one;
+    if(temp>epsmch && temp<eps)
+        err[i]=((LM_REAL)log10(temp) - epslog)/epslog;
+    if(temp>=eps) err[i]=zero;
+  }
+
+  free(buf);
+
+  return;
+}
+
+#ifdef HAVE_LAPACK
+/*
+ * This function computes the pseudoinverse of a square matrix A
+ * into B using SVD. A and B can coincide
+ * 
+ * The function returns 0 in case of error (e.g. A is singular),
+ * the rank of A if successful
+ *
+ * A, B are mxm
+ *
+ */
+static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m)
+{
+LM_REAL *buf=NULL;
+int buf_sz=0;
+static LM_REAL eps=LM_CNST(-1.0);
+
+register int i, j;
+LM_REAL *a, *u, *s, *vt, *work;
+int a_sz, u_sz, s_sz, vt_sz, tot_sz;
+LM_REAL thresh, one_over_denom;
+int info, rank, worksz, *iwork, iworksz;
+   
+  /* calculate required memory size */
+  worksz=5*m; // min worksize for GESVD
+  //worksz=m*(7*m+4); // min worksize for GESDD
+  iworksz=8*m;
+  a_sz=m*m;
+  u_sz=m*m; s_sz=m; vt_sz=m*m;
+
+  tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+    buf_sz=tot_sz;
+    buf=(LM_REAL *)malloc(buf_sz);
+    if(!buf){
+      fprintf(stderr, RCAT("memory allocation in ", LEVMAR_PSEUDOINVERSE) "() failed!\n");
+      return 0; /* error */
+    }
+
+  a=buf;
+  u=a+a_sz;
+  s=u+u_sz;
+  vt=s+s_sz;
+  work=vt+vt_sz;
+  iwork=(int *)(work+worksz);
+
+  /* store A (column major!) into a */
+  for(i=0; i<m; i++)
+    for(j=0; j<m; j++)
+      a[i+j*m]=A[i*m+j];
+
+  /* SVD decomposition of A */
+  GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
+  //GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
+
+  /* error treatment */
+  if(info!=0){
+    if(info<0){
+      fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", LEVMAR_PSEUDOINVERSE) "()\n", -info);
+    }
+    else{
+      fprintf(stderr, RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", LEVMAR_PSEUDOINVERSE) "() [info=%d]\n", info);
+    }
+    free(buf);
+    return 0;
+  }
+
+  if(eps<0.0){
+    LM_REAL aux;
+
+    /* compute machine epsilon */
+    for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
+                                          ;
+    eps*=LM_CNST(2.0);
+  }
+
+  /* compute the pseudoinverse in B */
+	for(i=0; i<a_sz; i++) B[i]=0.0; /* initialize to zero */
+  for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
+    one_over_denom=LM_CNST(1.0)/s[rank];
+
+    for(j=0; j<m; j++)
+      for(i=0; i<m; i++)
+        B[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
+  }
+
+  free(buf);
+
+	return rank;
+}
+#else // no LAPACK
+
+/*
+ * This function computes the inverse of A in B. A and B can coincide
+ *
+ * The function employs LAPACK-free LU decomposition of A to solve m linear
+ * systems A*B_i=I_i, where B_i and I_i are the i-th columns of B and I.
+ *
+ * A and B are mxm
+ *
+ * The function returns 0 in case of error, 1 if successful
+ *
+ */
+static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m)
+{
+void *buf=NULL;
+int buf_sz=0;
+
+register int i, j, k, l;
+int *idx, maxi=-1, idx_sz, a_sz, x_sz, work_sz, tot_sz;
+LM_REAL *a, *x, *work, max, sum, tmp;
+
+  /* calculate required memory size */
+  idx_sz=m;
+  a_sz=m*m;
+  x_sz=m;
+  work_sz=m;
+  tot_sz=(a_sz + x_sz + work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
+
+  buf_sz=tot_sz;
+  buf=(void *)malloc(tot_sz);
+  if(!buf){
+    fprintf(stderr, RCAT("memory allocation in ", LEVMAR_LUINVERSE) "() failed!\n");
+    return 0; /* error */
+  }
+
+  a=buf;
+  x=a+a_sz;
+  work=x+x_sz;
+  idx=(int *)(work+work_sz);
+
+  /* avoid destroying A by copying it to a */
+  for(i=0; i<a_sz; ++i) a[i]=A[i];
+
+  /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
+	for(i=0; i<m; ++i){
+		max=0.0;
+		for(j=0; j<m; ++j)
+			if((tmp=FABS(a[i*m+j]))>max)
+        max=tmp;
+		  if(max==0.0){
+        fprintf(stderr, RCAT("Singular matrix A in ", LEVMAR_LUINVERSE) "()!\n");
+        free(buf);
+
+        return 0;
+      }
+		  work[i]=LM_CNST(1.0)/max;
+	}
+
+	for(j=0; j<m; ++j){
+		for(i=0; i<j; ++i){
+			sum=a[i*m+j];
+			for(k=0; k<i; ++k)
+        sum-=a[i*m+k]*a[k*m+j];
+			a[i*m+j]=sum;
+		}
+		max=0.0;
+		for(i=j; i<m; ++i){
+			sum=a[i*m+j];
+			for(k=0; k<j; ++k)
+        sum-=a[i*m+k]*a[k*m+j];
+			a[i*m+j]=sum;
+			if((tmp=work[i]*FABS(sum))>=max){
+				max=tmp;
+				maxi=i;
+			}
+		}
+		if(j!=maxi){
+			for(k=0; k<m; ++k){
+				tmp=a[maxi*m+k];
+				a[maxi*m+k]=a[j*m+k];
+				a[j*m+k]=tmp;
+			}
+			work[maxi]=work[j];
+		}
+		idx[j]=maxi;
+		if(a[j*m+j]==0.0)
+      a[j*m+j]=LM_REAL_EPSILON;
+		if(j!=m-1){
+			tmp=LM_CNST(1.0)/(a[j*m+j]);
+			for(i=j+1; i<m; ++i)
+        a[i*m+j]*=tmp;
+		}
+	}
+
+  /* The decomposition has now replaced a. Solve the m linear systems using
+   * forward and back substitution
+   */
+  for(l=0; l<m; ++l){
+    for(i=0; i<m; ++i) x[i]=0.0;
+    x[l]=LM_CNST(1.0);
+
+	  for(i=k=0; i<m; ++i){
+		  j=idx[i];
+		  sum=x[j];
+		  x[j]=x[i];
+		  if(k!=0)
+			  for(j=k-1; j<i; ++j)
+          sum-=a[i*m+j]*x[j];
+		  else
+        if(sum!=0.0)
+			    k=i+1;
+		  x[i]=sum;
+	  }
+
+	  for(i=m-1; i>=0; --i){
+		  sum=x[i];
+		  for(j=i+1; j<m; ++j)
+        sum-=a[i*m+j]*x[j];
+		  x[i]=sum/a[i*m+i];
+	  }
+
+    for(i=0; i<m; ++i)
+      B[i*m+l]=x[i];
+  }
+
+  free(buf);
+
+  return 1;
+}
+#endif /* HAVE_LAPACK */
+
+/*
+ * This function computes in C the covariance matrix corresponding to a least
+ * squares fit. JtJ is the approximate Hessian at the solution (i.e. J^T*J, where
+ * J is the Jacobian at the solution), sumsq is the sum of squared residuals
+ * (i.e. goodnes of fit) at the solution, m is the number of parameters (variables)
+ * and n the number of observations. JtJ can coincide with C.
+ * 
+ * if JtJ is of full rank, C is computed as sumsq/(n-m)*(JtJ)^-1
+ * otherwise and if LAPACK is available, C=sumsq/(n-r)*(JtJ)^+
+ * where r is JtJ's rank and ^+ denotes the pseudoinverse
+ * The diagonal of C is made up from the estimates of the variances
+ * of the estimated regression coefficients.
+ * See the documentation of routine E04YCF from the NAG fortran lib
+ *
+ * The function returns the rank of JtJ if successful, 0 on error
+ *
+ * A and C are mxm
+ *
+ */
+int LEVMAR_COVAR(LM_REAL *JtJ, LM_REAL *C, LM_REAL sumsq, int m, int n)
+{
+register int i;
+int rnk;
+LM_REAL fact;
+
+#ifdef HAVE_LAPACK
+   rnk=LEVMAR_PSEUDOINVERSE(JtJ, C, m);
+   if(!rnk) return 0;
+#else
+#ifdef _MSC_VER
+#pragma message("LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times")
+#else
+#warning LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times
+#endif // _MSC_VER
+
+   rnk=LEVMAR_LUINVERSE(JtJ, C, m);
+   if(!rnk) return 0;
+
+   rnk=m; /* assume full rank */
+#endif /* HAVE_LAPACK */
+
+   fact=sumsq/(LM_REAL)(n-rnk);
+   for(i=0; i<m*m; ++i)
+     C[i]*=fact;
+
+   return rnk;
+}
+
+/*  standard deviation of the best-fit parameter i.
+ *  covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
+ *
+ *  The standard deviation is computed as \sigma_{i} = \sqrt{C_{ii}} 
+ */
+LM_REAL LEVMAR_STDDEV(LM_REAL *covar, int m, int i)
+{
+   return (LM_REAL)sqrt(covar[i*m+i]);
+}
+
+/* Pearson's correlation coefficient of the best-fit parameters i and j.
+ * covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
+ *
+ * The coefficient is computed as \rho_{ij} = C_{ij} / sqrt(C_{ii} C_{jj})
+ */
+LM_REAL LEVMAR_CORCOEF(LM_REAL *covar, int m, int i, int j)
+{
+   return (LM_REAL)(covar[i*m+j]/sqrt(covar[i*m+i]*covar[j*m+j]));
+}
+
+/* coefficient of determination.
+ * see  http://en.wikipedia.org/wiki/Coefficient_of_determination
+ */
+LM_REAL LEVMAR_R2(void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
+                  LM_REAL *p, LM_REAL *x, int m, int n, void *adata)
+{
+register int i;
+register LM_REAL tmp;
+LM_REAL SSerr,  // sum of squared errors, i.e. residual sum of squares \sum_i (x_i-hx_i)^2 
+        SStot, // \sum_i (x_i-xavg)^2
+        *hx, xavg;
+
+
+  if((hx=(LM_REAL *)malloc(n*sizeof(LM_REAL)))==NULL){
+    fprintf(stderr, RCAT("memory allocation request failed in ", LEVMAR_R2) "()\n");
+    exit(1);
+  }
+
+  /* hx=f(p) */
+  (*func)(p, hx, m, n, adata);
+
+  for(i=n, tmp=0.0; i-->0; )
+    tmp+=x[i];
+  xavg=tmp/(LM_REAL)n;
+  
+  if(x)
+    for(i=n, SSerr=SStot=0.0; i-->0; ){
+      tmp=x[i]-hx[i];
+      SSerr+=tmp*tmp;
+
+      tmp=x[i]-xavg;
+      SStot+=tmp*tmp;
+    }
+  else /* x==0 */
+    for(i=n, SSerr=SStot=0.0; i-->0; ){
+      tmp=-hx[i];
+      SSerr+=tmp*tmp;
+
+      tmp=-xavg;
+      SStot+=tmp*tmp;
+    }
+
+  free(hx);
+
+  return LM_CNST(1.0) - SSerr/SStot;
+}
+
+/* check box constraints for consistency */
+int LEVMAR_BOX_CHECK(LM_REAL *lb, LM_REAL *ub, int m)
+{
+register int i;
+
+  if(!lb || !ub) return 1;
+
+  for(i=0; i<m; ++i)
+    if(lb[i]>ub[i]) return 0;
+
+  return 1;
+}
+
+#ifdef HAVE_LAPACK
+
+/* compute the Cholesky decomposition of C in W, s.t. C=W^t W and W is upper triangular */
+int LEVMAR_CHOLESKY(LM_REAL *C, LM_REAL *W, int m)
+{
+register int i, j;
+int info;
+
+  /* copy weights array C to W so that LAPACK won't destroy it;
+   * C is assumed symmetric, hence no transposition is needed
+   */
+  for(i=0, j=m*m; i<j; ++i)
+    W[i]=C[i];
+
+  /* Cholesky decomposition */
+  POTF2("L", (int *)&m, W, (int *)&m, (int *)&info);
+  /* error treatment */
+  if(info!=0){
+		if(info<0){
+      fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_CHOLESKY, "()"));
+		}
+		else{
+			fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,
+						RCAT("and the Cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));
+		}
+    return LM_ERROR;
+  }
+
+  /* the decomposition is in the lower part of W (in column-major order!).
+   * zeroing the upper part makes it lower triangular which is equivalent to
+   * upper triangular in row-major order
+   */
+  for(i=0; i<m; i++)
+    for(j=i+1; j<m; j++)
+      W[i+j*m]=0.0;
+
+  return 0;
+}
+#endif /* HAVE_LAPACK */
+
+
+/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.
+ * e can coincide with either x or y; x can be NULL, in which case it is assumed
+ * to be equal to the zero vector.
+ * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline
+ * stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html
+ */
+
+LM_REAL LEVMAR_L2NRMXMY(LM_REAL *e, LM_REAL *x, LM_REAL *y, int n)
+{
+const int blocksize=8, bpwr=3; /* 8=2^3 */
+register int i;
+int j1, j2, j3, j4, j5, j6, j7;
+int blockn;
+register LM_REAL sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;
+
+  /* n may not be divisible by blocksize, 
+   * go as near as we can first, then tidy up.
+   */ 
+  blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */
+
+  /* unroll the loop in blocks of `blocksize'; looping downwards gains some more speed */
+  if(x){
+    for(i=blockn-1; i>0; i-=blocksize){
+              e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ];
+      j1=i-1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1];
+      j2=i-2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2];
+      j3=i-3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3];
+      j4=i-4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4];
+      j5=i-5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5];
+      j6=i-6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6];
+      j7=i-7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7];
+    }
+
+   /*
+    * There may be some left to do.
+    * This could be done as a simple for() loop, 
+    * but a switch is faster (and more interesting) 
+    */ 
+
+    i=blockn;
+    if(i<n){ 
+      /* Jump into the case at the place that will allow
+       * us to finish off the appropriate number of items. 
+       */ 
+
+      switch(n - i){ 
+        case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
+        case 6 : e[i]=x[i]-y[i]; sum1+=e[i]*e[i]; ++i;
+        case 5 : e[i]=x[i]-y[i]; sum2+=e[i]*e[i]; ++i;
+        case 4 : e[i]=x[i]-y[i]; sum3+=e[i]*e[i]; ++i;
+        case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
+        case 2 : e[i]=x[i]-y[i]; sum1+=e[i]*e[i]; ++i;
+        case 1 : e[i]=x[i]-y[i]; sum2+=e[i]*e[i]; //++i;
+      }
+    }
+  }
+  else{ /* x==0 */
+    for(i=blockn-1; i>0; i-=blocksize){
+              e[i ]=-y[i ]; sum0+=e[i ]*e[i ];
+      j1=i-1; e[j1]=-y[j1]; sum1+=e[j1]*e[j1];
+      j2=i-2; e[j2]=-y[j2]; sum2+=e[j2]*e[j2];
+      j3=i-3; e[j3]=-y[j3]; sum3+=e[j3]*e[j3];
+      j4=i-4; e[j4]=-y[j4]; sum0+=e[j4]*e[j4];
+      j5=i-5; e[j5]=-y[j5]; sum1+=e[j5]*e[j5];
+      j6=i-6; e[j6]=-y[j6]; sum2+=e[j6]*e[j6];
+      j7=i-7; e[j7]=-y[j7]; sum3+=e[j7]*e[j7];
+    }
+
+   /*
+    * There may be some left to do.
+    * This could be done as a simple for() loop, 
+    * but a switch is faster (and more interesting) 
+    */ 
+
+    i=blockn;
+    if(i<n){ 
+      /* Jump into the case at the place that will allow
+       * us to finish off the appropriate number of items. 
+       */ 
+
+      switch(n - i){ 
+        case 7 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
+        case 6 : e[i]=-y[i]; sum1+=e[i]*e[i]; ++i;
+        case 5 : e[i]=-y[i]; sum2+=e[i]*e[i]; ++i;
+        case 4 : e[i]=-y[i]; sum3+=e[i]*e[i]; ++i;
+        case 3 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
+        case 2 : e[i]=-y[i]; sum1+=e[i]*e[i]; ++i;
+        case 1 : e[i]=-y[i]; sum2+=e[i]*e[i]; //++i;
+      }
+    }
+  }
+
+  return sum0+sum1+sum2+sum3;
+}
+
+/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
+#undef POTF2
+#undef GESVD
+#undef GESDD
+#undef GEMM
+#undef LEVMAR_PSEUDOINVERSE
+#undef LEVMAR_LUINVERSE
+#undef LEVMAR_BOX_CHECK
+#undef LEVMAR_CHOLESKY
+#undef LEVMAR_COVAR
+#undef LEVMAR_STDDEV
+#undef LEVMAR_CORCOEF
+#undef LEVMAR_R2
+#undef LEVMAR_CHKJAC
+#undef LEVMAR_FDIF_FORW_JAC_APPROX
+#undef LEVMAR_FDIF_CENT_JAC_APPROX
+#undef LEVMAR_TRANS_MAT_MAT_MULT
+#undef LEVMAR_L2NRMXMY
