/////////////////////////////////////////////////////////////////////////////////
//// 
////  Simple drivers for sparse bundle adjustment based on the
////  Levenberg - Marquardt minimization algorithm
////  This file provides simple wrappers to the functions defined in sba_levmar.c
////  Copyright (C) 2004  Manolis Lourakis (lourakis@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 <string.h>
#include <math.h>
#include <float.h>

#include "sba.h"


#define FABS(x)           (((x)>=0)? (x) : -(x))

struct wrap_motstr_data_ {
  void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata); // Q
  void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata); // dQ/da, dQ/db
  int cnp, pnp, mnp; /* parameter numbers */
  void *adata;
};

struct wrap_mot_data_ {
  void (*proj)(int j, int i, double *aj, double *xij, void *adata); // Q
  void (*projac)(int j, int i, double *aj, double *Aij, void *adata); // dQ/da
  int cnp, mnp; /* parameter numbers */
  void *adata;
};

struct wrap_str_data_ {
  void (*proj)(int j, int i, double *bi, double *xij, void *adata); // Q
  void (*projac)(int j, int i, double *bi, double *Bij, void *adata); // dQ/db
  int pnp, mnp; /* parameter numbers */
  void *adata;
};

/* Routines to estimate the estimated measurement vector (i.e. "func") and
 * its sparse jacobian (i.e. "fjac") needed by BA expert drivers. Code below
 * makes use of user-supplied functions computing "Q", "dQ/da", d"Q/db",
 * i.e. predicted projection and associated jacobians for a SINGLE image measurement.
 * Notice also that what follows is two pairs of "func" and corresponding "fjac" routines.
 * The first is to be used in full (i.e. motion + structure) BA, the second in 
 * motion only BA.
 */

/* FULL BUNDLE ADJUSTMENT */

/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in
 * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements
 * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted
 * projection of the i-th point on the j-th camera.
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the hx_ij might be missing
 *
 */
static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)
{
  register int i, j;
  int cnp, pnp, mnp; 
  double *pa, *pb, *paj, *pbi, *pxij;
  int n, m, nnz;
  struct wrap_motstr_data_ *wdata;
  void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata);
  void *proj_adata;

  wdata=(struct wrap_motstr_data_ *)adata;
  cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp;
  proj=wdata->proj;
  proj_adata=wdata->adata;

  n=idxij->nr; m=idxij->nc;
  pa=p; pb=p+m*cnp;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    paj=pa+j*cnp;

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      pbi=pb + rcsubs[i]*pnp;
      pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij

      (*proj)(j, rcsubs[i], paj, pbi, pxij, proj_adata); // evaluate Q in pxij
    }
  }
}

/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in
 * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm, B_11, ..., B_1m, ..., B_n1, ..., B_nm),
 * where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ).
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the A_ij, B_ij might be missing
 *
 */
static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
  register int i, j;
  int cnp, pnp, mnp;
  double *pa, *pb, *paj, *pbi, *jaca, *jacb, *pAij, *pBij;
  int n, m, nnz, Asz, Bsz, idx;
  struct wrap_motstr_data_ *wdata;
  void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata);
  void *projac_adata;

  
  wdata=(struct wrap_motstr_data_ *)adata;
  cnp=wdata->cnp; pnp=wdata->pnp; mnp=wdata->mnp;
  projac=wdata->projac;
  projac_adata=wdata->adata;

  n=idxij->nr; m=idxij->nc;
  pa=p; pb=p+m*cnp;
  Asz=mnp*cnp; Bsz=mnp*pnp;
  jaca=jac; jacb=jac+idxij->nnz*Asz;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    paj=pa+j*cnp;

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      pbi=pb + rcsubs[i]*pnp;
      idx=idxij->val[rcidxs[i]];
      pAij=jaca + idx*Asz; // set pAij to point to A_ij
      pBij=jacb + idx*Bsz; // set pBij to point to B_ij

      (*projac)(j, rcsubs[i], paj, pbi, pAij, pBij, projac_adata); // evaluate dQ/da, dQ/db in pAij, pBij
    }
  }
}

/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in
 * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is approximated with the aid of finite differences and is returned in the order
 * (A_11, ..., A_1m, ..., A_n1, ..., A_nm, B_11, ..., B_1m, ..., B_n1, ..., B_nm),
 * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ).
 * Notice that depending on idxij, some of the A_ij, B_ij might be missing
 *
 * Problem-specific information is assumed to be stored in a structure pointed to by "dat".
 *
 * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,
 * the jacobian should be computed analytically
 */
static void sba_motstr_Qs_fdjac(
    double *p,                /* I: current parameter estimate, (m*cnp+n*pnp)x1 */
    struct sba_crsm *idxij,   /* I: sparse matrix containing the location of x_ij in hx */
    int    *rcidxs,           /* work array for the indexes of nonzero elements of a single sparse matrix row/column */
    int    *rcsubs,           /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */
    double *jac,              /* O: array for storing the approximated jacobian */
    void   *dat)              /* I: points to a "wrap_motstr_data_" structure */
{
  register int i, j, ii, jj;
  double *pa, *pb, *paj, *pbi, *jaca, *jacb;
  register double *pAB;
  int n, m, nnz, Asz, Bsz;

  double tmp;
  register double d, d1;

  struct wrap_motstr_data_ *fdjd;
  void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata);
  double *hxij, *hxxij;
  int cnp, pnp, mnp;
  void *adata;

  /* retrieve problem-specific information passed in *dat */
  fdjd=(struct wrap_motstr_data_ *)dat;
  proj=fdjd->proj;
  cnp=fdjd->cnp; pnp=fdjd->pnp; mnp=fdjd->mnp;
  adata=fdjd->adata;

  n=idxij->nr; m=idxij->nc;
  pa=p; pb=p+m*cnp;
  Asz=mnp*cnp; Bsz=mnp*pnp;
  jaca=jac; jacb=jac+idxij->nnz*Asz;

  /* allocate memory for hxij, hxxij */
  if((hxij=malloc(2*mnp*sizeof(double)))==NULL){
    fprintf(stderr, "memory allocation request failed in sba_motstr_Qs_fdjac()!\n");
    exit(1);
  }
  hxxij=hxij+mnp;

  if(cnp){ // is motion varying?
    /* compute A_ij */
    for(j=0; j<m; ++j){
      paj=pa+j*cnp; // j-th camera parameters

      nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */
      for(jj=0; jj<cnp; ++jj){
        /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */
        d=(double)(SBA_DELTA_SCALE)*paj[jj]; // force evaluation
        d=FABS(d);
        if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
        d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */

        for(i=0; i<nnz; ++i){
          pbi=pb + rcsubs[i]*pnp; // i-th point parameters
          (*proj)(j, rcsubs[i], paj, pbi, hxij, adata); // evaluate supplied function on current solution

          tmp=paj[jj];
          paj[jj]+=d;
          (*proj)(j, rcsubs[i], paj, pbi, hxxij, adata);
          paj[jj]=tmp; /* restore */

          pAB=jaca + idxij->val[rcidxs[i]]*Asz; // set pAB to point to A_ij
          for(ii=0; ii<mnp; ++ii)
            pAB[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1;
        }
      }
    }
  }

  if(pnp){ // is structure varying?
    /* compute B_ij */
    for(i=0; i<n; ++i){
      pbi=pb+i*pnp; // i-th point parameters

      nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */
      for(jj=0; jj<pnp; ++jj){
        /* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */
        d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation
        d=FABS(d);
        if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
        d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */

        for(j=0; j<nnz; ++j){
          paj=pa + rcsubs[j]*cnp; // j-th camera parameters
          (*proj)(rcsubs[j], i, paj, pbi, hxij, adata); // evaluate supplied function on current solution

          tmp=pbi[jj];
          pbi[jj]+=d;
          (*proj)(rcsubs[j], i, paj, pbi, hxxij, adata);
          pbi[jj]=tmp; /* restore */

          pAB=jacb + idxij->val[rcidxs[j]]*Bsz; // set pAB to point to B_ij
          for(ii=0; ii<mnp; ++ii)
            pAB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;
        }
      }
    }
  }

  free(hxij);
}

/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */

/* Given a parameter vector p made up of the parameters of m cameras, compute in
 * hx the prediction of the measurements, i.e. the projections of 3D points in the m images.
 * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T,
 * where hx_ij is the predicted projection of the i-th point on the j-th camera.
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the hx_ij might be missing
 *
 */
static void sba_mot_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)
{
  register int i, j;
  int cnp, mnp; 
  double *paj, *pxij;
  //int n;
  int m, nnz;
  struct wrap_mot_data_ *wdata;
  void (*proj)(int j, int i, double *aj, double *xij, void *proj_adata);
  void *proj_adata;

  wdata=(struct wrap_mot_data_ *)adata;
  cnp=wdata->cnp; mnp=wdata->mnp;
  proj=wdata->proj;
  proj_adata=wdata->adata;

  //n=idxij->nr;
  m=idxij->nc;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    paj=p+j*cnp;

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij

      (*proj)(j, rcsubs[i], paj, pxij, proj_adata); // evaluate Q in pxij
    }
  }
}

/* Given a parameter vector p made up of the parameters of m cameras, compute in jac
 * the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm),
 * where A_ij=dx_ij/db_j (see HZ).
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the A_ij might be missing
 *
 */
static void sba_mot_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
  register int i, j;
  int cnp, mnp;
  double *paj, *jaca, *pAij;
  //int n;
  int m, nnz, Asz, idx;
  struct wrap_mot_data_ *wdata;
  void (*projac)(int j, int i, double *aj, double *Aij, void *projac_adata);
  void *projac_adata;

  wdata=(struct wrap_mot_data_ *)adata;
  cnp=wdata->cnp; mnp=wdata->mnp;
  projac=wdata->projac;
  projac_adata=wdata->adata;

  //n=idxij->nr;
  m=idxij->nc;
  Asz=mnp*cnp;
  jaca=jac;

  for(j=0; j<m; ++j){
    /* j-th camera parameters */
    paj=p+j*cnp;

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      idx=idxij->val[rcidxs[i]];
      pAij=jaca + idx*Asz; // set pAij to point to A_ij

      (*projac)(j, rcsubs[i], paj, pAij, projac_adata); // evaluate dQ/da in pAij
    }
  }
}

/* Given a parameter vector p made up of the parameters of m cameras, compute in jac the jacobian
 * of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is approximated with the aid of finite differences and is returned in the order
 * (A_11, ..., A_1m, ..., A_n1, ..., A_nm), where A_ij=dx_ij/da_j (see HZ).
 * Notice that depending on idxij, some of the A_ij might be missing
 *
 * Problem-specific information is assumed to be stored in a structure pointed to by "dat".
 *
 * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,
 * the jacobian should be computed analytically
 */
static void sba_mot_Qs_fdjac(
    double *p,                /* I: current parameter estimate, (m*cnp)x1 */
    struct sba_crsm *idxij,   /* I: sparse matrix containing the location of x_ij in hx */
    int    *rcidxs,           /* work array for the indexes of nonzero elements of a single sparse matrix row/column */
    int    *rcsubs,           /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */
    double *jac,              /* O: array for storing the approximated jacobian */
    void   *dat)              /* I: points to a "wrap_mot_data_" structure */
{
  register int i, j, ii, jj;
  double *paj, *jaca;
  register double *pA;
  //int n; 
  int m, nnz, Asz;

  double tmp;
  register double d, d1;

  struct wrap_mot_data_ *fdjd;
  void (*proj)(int j, int i, double *aj, double *xij, void *adata);
  double *hxij, *hxxij;
  int cnp, mnp;
  void *adata;

  /* retrieve problem-specific information passed in *dat */
  fdjd=(struct wrap_mot_data_ *)dat;
  proj=fdjd->proj;
  cnp=fdjd->cnp; mnp=fdjd->mnp;
  adata=fdjd->adata;

  //n=idxij->nr;
  m=idxij->nc;
  Asz=mnp*cnp;
  jaca=jac;

  /* allocate memory for hxij, hxxij */
  if((hxij=malloc(2*mnp*sizeof(double)))==NULL){
    fprintf(stderr, "memory allocation request failed in sba_mot_Qs_fdjac()!\n");
    exit(1);
  }
  hxxij=hxij+mnp;

  /* compute A_ij */
  for(j=0; j<m; ++j){
    paj=p+j*cnp; // j-th camera parameters

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero A_ij, i=0...n-1 */
    for(jj=0; jj<cnp; ++jj){
      /* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */
      d=(double)(SBA_DELTA_SCALE)*paj[jj]; // force evaluation
      d=FABS(d);
      if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
      d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */

      for(i=0; i<nnz; ++i){
        (*proj)(j, rcsubs[i], paj, hxij, adata); // evaluate supplied function on current solution

        tmp=paj[jj];
        paj[jj]+=d;
        (*proj)(j, rcsubs[i], paj, hxxij, adata);
        paj[jj]=tmp; /* restore */

        pA=jaca + idxij->val[rcidxs[i]]*Asz; // set pA to point to A_ij
        for(ii=0; ii<mnp; ++ii)
          pA[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1;
      }
    }
  }

  free(hxij);
}

/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */

/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
 * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements
 * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted
 * projection of the i-th point on the j-th camera.
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the hx_ij might be missing
 *
 */
static void sba_str_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata)
{
  register int i, j;
  int pnp, mnp; 
  double *pbi, *pxij;
  //int n;
  int m, nnz;
  struct wrap_str_data_ *wdata;
  void (*proj)(int j, int i, double *bi, double *xij, void *proj_adata);
  void *proj_adata;

  wdata=(struct wrap_str_data_ *)adata;
  pnp=wdata->pnp; mnp=wdata->mnp;
  proj=wdata->proj;
  proj_adata=wdata->adata;

  //n=idxij->nr;
  m=idxij->nc;

  for(j=0; j<m; ++j){
    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      pbi=p + rcsubs[i]*pnp;
      pxij=hx + idxij->val[rcidxs[i]]*mnp; // set pxij to point to hx_ij

      (*proj)(j, rcsubs[i], pbi, pxij, proj_adata); // evaluate Q in pxij
    }
  }
}

/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
 * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).
 * Caller supplies rcidxs and rcsubs which can be used as working memory.
 * Notice that depending on idxij, some of the B_ij might be missing
 *
 */
static void sba_str_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata)
{
  register int i, j;
  int pnp, mnp;
  double *pbi, *pBij;
  //int n;
  int m, nnz, Bsz, idx;
  struct wrap_str_data_ *wdata;
  void (*projac)(int j, int i, double *bi, double *Bij, void *projac_adata);
  void *projac_adata;

  
  wdata=(struct wrap_str_data_ *)adata;
  pnp=wdata->pnp; mnp=wdata->mnp;
  projac=wdata->projac;
  projac_adata=wdata->adata;

  //n=idxij->nr;
  m=idxij->nc;
  Bsz=mnp*pnp;

  for(j=0; j<m; ++j){

    nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); /* find nonzero hx_ij, i=0...n-1 */

    for(i=0; i<nnz; ++i){
      pbi=p + rcsubs[i]*pnp;
      idx=idxij->val[rcidxs[i]];
      pBij=jac + idx*Bsz; // set pBij to point to B_ij

      (*projac)(j, rcsubs[i], pbi, pBij, projac_adata); // evaluate dQ/db in pBij
    }
  }
}

/* Given a parameter vector p made up of the 3D coordinates of n points, compute in
 * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.
 * The jacobian is approximated with the aid of finite differences and is returned in the order
 * (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).
 * Notice that depending on idxij, some of the B_ij might be missing
 *
 * Problem-specific information is assumed to be stored in a structure pointed to by "dat".
 *
 * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,
 * the jacobian should be computed analytically
 */
static void sba_str_Qs_fdjac(
    double *p,                /* I: current parameter estimate, (n*pnp)x1 */
    struct sba_crsm *idxij,   /* I: sparse matrix containing the location of x_ij in hx */
    int    *rcidxs,           /* work array for the indexes of nonzero elements of a single sparse matrix row/column */
    int    *rcsubs,           /* work array for the subscripts of nonzero elements in a single sparse matrix row/column */
    double *jac,              /* O: array for storing the approximated jacobian */
    void   *dat)              /* I: points to a "wrap_str_data_" structure */
{
  register int i, j, ii, jj;
  double *pbi;
  register double *pB;
  //int m;
  int n, nnz, Bsz;

  double tmp;
  register double d, d1;

  struct wrap_str_data_ *fdjd;
  void (*proj)(int j, int i, double *bi, double *xij, void *adata);
  double *hxij, *hxxij;
  int pnp, mnp;
  void *adata;

  /* retrieve problem-specific information passed in *dat */
  fdjd=(struct wrap_str_data_ *)dat;
  proj=fdjd->proj;
  pnp=fdjd->pnp; mnp=fdjd->mnp;
  adata=fdjd->adata;

  n=idxij->nr;
  //m=idxij->nc;
  Bsz=mnp*pnp;

  /* allocate memory for hxij, hxxij */
  if((hxij=malloc(2*mnp*sizeof(double)))==NULL){
    fprintf(stderr, "memory allocation request failed in sba_str_Qs_fdjac()!\n");
    exit(1);
  }
  hxxij=hxij+mnp;

  /* compute B_ij */
  for(i=0; i<n; ++i){
    pbi=p+i*pnp; // i-th point parameters

    nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero B_ij, j=0...m-1 */
    for(jj=0; jj<pnp; ++jj){
      /* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */
      d=(double)(SBA_DELTA_SCALE)*pbi[jj]; // force evaluation
      d=FABS(d);
      if(d<SBA_MIN_DELTA) d=SBA_MIN_DELTA;
      d1=1.0/d; /* invert so that divisions can be carried out faster as multiplications */

      for(j=0; j<nnz; ++j){
        (*proj)(rcsubs[j], i, pbi, hxij, adata); // evaluate supplied function on current solution

        tmp=pbi[jj];
        pbi[jj]+=d;
        (*proj)(rcsubs[j], i, pbi, hxxij, adata);
        pbi[jj]=tmp; /* restore */

        pB=jac + idxij->val[rcidxs[j]]*Bsz; // set pB to point to B_ij
        for(ii=0; ii<mnp; ++ii)
          pB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;
      }
    }
  }

  free(hxij);
}


/* 
 * Simple driver to sba_motstr_levmar_x for bundle adjustment on camera and structure parameters.
 */

int sba_motstr_levmar(
    const int n,   /* number of points */
    const int m,   /* number of images */
    const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified.
					          * All A_ij (see below) with j<mcon are assumed to be zero
					          */
    char *vmask,  /* visibility mask: vmask[i][j]=1 if point i visible in image j, 0 otherwise. nxm */
    double *p,    /* initial parameter vector p0: (a1, ..., am, b1, ..., bn).
                   * aj are the image j parameters, bi are the i-th point parameters,
                   * size m*cnp + n*pnp
                   */
    const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */
    const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */
    double *x,    /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where
                   * x_ij is the projection of the i-th point on the j-th image.
                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;
                   * see vmask[i][j], max. size n*m*mnp
                   */
    const int mnp,/* number of parameters for EACH measurement; usually 2 */
    void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *adata),
                                              /* functional relation computing a SINGLE image measurement. Assuming that
                                               * the parameters of point i are bi and the parameters of camera j aj,
                                               * computes a prediction of \hat{x}_{ij}. aj is cnp x 1, bi is pnp x 1 and
                                               * xij is mnp x 1. This function is called only if point i is visible in
                                               * image j (i.e. vmask[i][j]==1)
                                               */
    void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *adata),
                                              /* functional relation to evaluate d x_ij / d a_j and
                                               * d x_ij / d b_i in Aij and Bij resp.
                                               * This function is called only if point i is visible in * image j
                                               * (i.e. vmask[i][j]==1). Also, A_ij and B_ij are mnp x cnp and mnp x pnp
                                               * matrices resp. and they should be stored in row-major order.
                                               *
                                               * If NULL, the jacobians are approximated by repetitive proj calls
                                               * and finite differences. 
                                               */
    void *adata,       /* pointer to possibly additional data, passed uninterpreted to proj, projac */ 

    int itmax,         /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */
    int verbose,       /* I: verbosity */
    double opts[SBA_OPTSSZ],
	                     /* I: minim. options [\mu, \epsilon1, \epsilon2]. Respectively the scale factor for initial \mu,
                        * stoping thresholds for ||J^T e||_inf, ||dp||_2 and ||e||_2
                        */
    double info[SBA_INFOSZ]
	                     /* 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 - too many attempts to increase damping. Restart with increased mu
                        *                                 6 - stopped by small ||e||_2
                        * info[7]= # function evaluations
                        * info[8]= # jacobian evaluations
			                  * info[9]= # number of linear systems solved, i.e. number of attempts	for reducing error
                        */
)
{
int retval;
struct wrap_motstr_data_ wdata;
static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata);

  wdata.proj=proj;
  wdata.projac=projac;
  wdata.cnp=cnp;
  wdata.pnp=pnp;
  wdata.mnp=mnp;
  wdata.adata=adata;

  fjac=(projac)? sba_motstr_Qs_jac : sba_motstr_Qs_fdjac;
  retval=sba_motstr_levmar_x(n, m, mcon, vmask, p, cnp, pnp, x, mnp, sba_motstr_Qs, fjac, &wdata, itmax, verbose, opts, info);

  if(info){
    register int i;
    int nvis;

    /* count visible image points */
    for(i=nvis=0; i<n*m; ++i)
      nvis+=vmask[i];

    /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */
    info[7]*=nvis;
    info[8]*=nvis;
  }

  return retval;
}


/* 
 * Simple driver to sba_mot_levmar_x for bundle adjustment on camera parameters.
 */

int sba_mot_levmar(
    const int n,   /* number of points */
    const int m,   /* number of images */
    const int mcon,/* number of images (starting from the 1st) whose parameters should not be modified.
					          * All A_ij (see below) with j<mcon are assumed to be zero
					          */
    char *vmask,  /* visibility mask: vmask[i][j]=1 if point i visible in image j, 0 otherwise. nxm */
    double *p,    /* initial parameter vector p0: (a1, ..., am).
                   * aj are the image j parameters, size m*cnp */
    const int cnp,/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */
    double *x,    /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where
                   * x_ij is the projection of the i-th point on the j-th image.
                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;
                   * see vmask[i][j], max. size n*m*mnp
                   */
    const int mnp,/* number of parameters for EACH measurement; usually 2 */
    void (*proj)(int j, int i, double *aj, double *xij, void *adata),
                                              /* functional relation computing a SINGLE image measurement. Assuming that
                                               * the parameters of camera j are aj, computes a prediction of \hat{x}_{ij}
                                               * for point i. aj is cnp x 1 and xij is mnp x 1.
                                               * This function is called only if point i is visible in  image j (i.e. vmask[i][j]==1)
                                               */
    void (*projac)(int j, int i, double *aj, double *Aij, void *adata),
                                              /* functional relation to evaluate d x_ij / d a_j in Aij 
                                               * This function is called only if point i is visible in image j
                                               * (i.e. vmask[i][j]==1). Also, A_ij are a mnp x cnp matrices
                                               * and should be stored in row-major order.
                                               *
                                               * If NULL, the jacobian is approximated by repetitive proj calls
                                               * and finite differences. 
                                               */
    void *adata,       /* pointer to possibly additional data, passed uninterpreted to proj, projac */ 

    int itmax,         /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */
    int verbose,       /* I: verbosity */
    double opts[SBA_OPTSSZ],
	                     /* I: minim. options [\mu, \epsilon1, \epsilon2]. Respectively the scale factor for initial \mu,
                        * stoping thresholds for ||J^T e||_inf, ||dp||_2 and ||e||_2
                        */
    double info[SBA_INFOSZ]
	                     /* 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 - too many attempts to increase damping. Restart with increased mu
                        *                                 6 - stopped by small ||e||_2
                        * info[7]= # function evaluations
                        * info[8]= # jacobian evaluations
			                  * info[9]= # number of linear systems solved, i.e. number of attempts	for reducing error
                        */
)
{
int retval;
struct wrap_mot_data_ wdata;
void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata);

  wdata.proj=proj;
  wdata.projac=projac;
  wdata.cnp=cnp;
  wdata.mnp=mnp;
  wdata.adata=adata;

  fjac=(projac)? sba_mot_Qs_jac : sba_mot_Qs_fdjac;
  retval=sba_mot_levmar_x(n, m, mcon, vmask, p, cnp, x, mnp, sba_mot_Qs, fjac, &wdata, itmax, verbose, opts, info);

  if(info){
    register int i;
    int nvis;

    /* count visible image points */
    for(i=nvis=0; i<n*m; ++i)
      nvis+=vmask[i];

    /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */
    info[7]*=nvis;
    info[8]*=nvis;
  }

  return retval;
}

/* 
 * Simple driver to sba_str_levmar_x for bundle adjustment on structure parameters.
 */

int sba_str_levmar(
    const int n,   /* number of points */
    const int m,   /* number of images */
    char *vmask,  /* visibility mask: vmask[i][j]=1 if point i visible in image j, 0 otherwise. nxm */
    double *p,    /* initial parameter vector p0: (b1, ..., bn).
                   * bi are the i-th point parameters, size n*pnp
                   */
    const int pnp,/* number of parameters for ONE point; e.g. 3 for Euclidean points */
    double *x,    /* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where
                   * x_ij is the projection of the i-th point on the j-th image.
                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;
                   * see vmask[i][j], max. size n*m*mnp
                   */
    const int mnp,/* number of parameters for EACH measurement; usually 2 */
    void (*proj)(int j, int i, double *bi, double *xij, void *adata),
                                              /* functional relation computing a SINGLE image measurement. Assuming that
                                               * the parameters of point i are bi, computes a prediction of \hat{x}_{ij}.
                                               * bi is pnp x 1 and  xij is mnp x 1. This function is called only if point
                                               * i is visible in image j (i.e. vmask[i][j]==1)
                                               */
    void (*projac)(int j, int i, double *bi, double *Bij, void *adata),
                                              /* functional relation to evaluate d x_ij / d b_i in Bij.
                                               * This function is called only if point i is visible in image j
                                               * (i.e. vmask[i][j]==1). Also, B_ij are mnp x pnp matrices
                                               * and they should be stored in row-major order.
                                               *
                                               * If NULL, the jacobians are approximated by repetitive proj calls
                                               * and finite differences. 
                                               */
    void *adata,       /* pointer to possibly additional data, passed uninterpreted to proj, projac */ 

    int itmax,         /* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */
    int verbose,       /* I: verbosity */
    double opts[SBA_OPTSSZ],
	                     /* I: minim. options [\mu, \epsilon1, \epsilon2]. Respectively the scale factor for initial \mu,
                        * stoping thresholds for ||J^T e||_inf, ||dp||_2 and ||e||_2
                        */
    double info[SBA_INFOSZ]
	                     /* 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 - too many attempts to increase damping. Restart with increased mu
                        *                                 6 - stopped by small ||e||_2
                        * info[7]= # function evaluations
                        * info[8]= # jacobian evaluations
			                  * info[9]= # number of linear systems solved, i.e. number of attempts	for reducing error
                        */
)
{
int retval;
struct wrap_str_data_ wdata;
static void (*fjac)(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata);

  wdata.proj=proj;
  wdata.projac=projac;
  wdata.pnp=pnp;
  wdata.mnp=mnp;
  wdata.adata=adata;

  fjac=(projac)? sba_str_Qs_jac : sba_str_Qs_fdjac;
  retval=sba_str_levmar_x(n, m, vmask, p, pnp, x, mnp, sba_str_Qs, fjac, &wdata, itmax, verbose, opts, info);

  if(info){
    register int i;
    int nvis;

    /* count visible image points */
    for(i=nvis=0; i<n*m; ++i)
      nvis+=vmask[i];

    /* each "func" & "fjac" evaluation requires nvis "proj" & "projac" evaluations */
    info[7]*=nvis;
    info[8]*=nvis;
  }

  return retval;
}