Merge with trunk r41625

5 files changed, 2601 insertions, 0 deletions

diff --git a/extern/libmv/third_party/ssba/Math/v3d_linear.h b/extern/libmv/third_party/ssba/Math/v3d_linear.h
new file mode 100644
index 00000000000..7d6e898169c
--- /dev/null
+++ b/extern/libmv/third_party/ssba/Math/v3d_linear.h
@@ -0,0 +1,923 @@
+// -*- C++ -*-
+/*
+Copyright (c) 2008 University of North Carolina at Chapel Hill
+
+This file is part of SSBA (Simple Sparse Bundle Adjustment).
+
+SSBA is free software: you can redistribute it and/or modify it under the
+terms of the GNU Lesser General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option) any
+later version.
+
+SSBA 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 Lesser General Public License for more
+details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with SSBA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef V3D_LINEAR_H
+#define V3D_LINEAR_H
+
+#include <cassert>
+#include <algorithm>
+#include <vector>
+#include <cmath>
+
+namespace V3D
+{
+   using namespace std;
+
+   //! Unboxed vector type
+   template <typename Elem, int Size>
+   struct InlineVectorBase
+   {
+         typedef Elem value_type;
+         typedef Elem element_type;
+
+         typedef Elem const * const_iterator;
+         typedef Elem       * iterator;
+
+         static unsigned int size() { return Size; }
+
+         Elem& operator[](unsigned int i)       { return _vec[i]; }
+         Elem  operator[](unsigned int i) const { return _vec[i]; }
+
+         Elem& operator()(unsigned int i)       { return _vec[i-1]; }
+         Elem  operator()(unsigned int i) const { return _vec[i-1]; }
+
+         const_iterator begin() const { return _vec; }
+         iterator       begin()       { return _vec; }
+         const_iterator end() const { return _vec + Size; }
+         iterator       end()       { return _vec + Size; }
+
+         void newsize(unsigned int sz) const
+         {
+            assert(sz == Size);
+         }
+
+      protected:
+         Elem _vec[Size];
+   };
+
+   //! Boxed (heap allocated) vector.
+   template <typename Elem>
+   struct VectorBase
+   {
+         typedef Elem value_type;
+         typedef Elem element_type;
+
+         typedef Elem const * const_iterator;
+         typedef Elem       * iterator;
+
+         VectorBase()
+            : _size(0), _ownsVec(true), _vec(0)
+         { }
+
+         VectorBase(unsigned int size)
+            : _size(size), _ownsVec(true), _vec(0)
+         {
+            if (size > 0) _vec = new Elem[size];
+         }
+
+         VectorBase(unsigned int size, Elem * values)
+            : _size(size), _ownsVec(false), _vec(values)
+         { }
+
+         VectorBase(VectorBase<Elem> const& a)
+            : _size(0), _ownsVec(true), _vec(0)
+         {
+            _size = a._size;
+            if (_size == 0) return;
+            _vec = new Elem[_size];
+            std::copy(a._vec, a._vec + _size, _vec);
+         }
+
+         ~VectorBase() { if (_ownsVec && _vec != 0) delete [] _vec; }
+
+         VectorBase& operator=(VectorBase<Elem> const& a)
+         {
+            if (this == &a) return *this;
+
+            this->newsize(a._size);
+            std::copy(a._vec, a._vec + _size, _vec);
+            return *this;
+         }
+
+         unsigned int size() const { return _size; }
+
+         VectorBase<Elem>& newsize(unsigned int sz)
+         {
+            if (sz == _size) return *this;
+            assert(_ownsVec);
+
+            __destroy();
+            _size = sz;
+            if (_size > 0) _vec = new Elem[_size];
+
+            return *this;
+         }
+
+
+         Elem& operator[](unsigned int i)       { return _vec[i]; }
+         Elem  operator[](unsigned int i) const { return _vec[i]; }
+
+         Elem& operator()(unsigned int i)       { return _vec[i-1]; }
+         Elem  operator()(unsigned int i) const { return _vec[i-1]; }
+
+         const_iterator begin() const { return _vec; }
+         iterator       begin()       { return _vec; }
+         const_iterator end() const { return _vec + _size; }
+         iterator       end()       { return _vec + _size; }
+
+      protected:
+         void __destroy()
+         {
+            assert(_ownsVec);
+
+            if (_vec != 0) delete [] _vec;
+            _size = 0;
+            _vec  = 0;
+         }
+
+         unsigned int _size;
+         bool         _ownsVec;
+         Elem       * _vec;
+   };
+
+   template <typename Elem, int Rows, int Cols>
+   struct InlineMatrixBase
+   {
+         typedef Elem         value_type;
+         typedef Elem         element_type;
+
+         typedef Elem       * iterator;
+         typedef Elem const * const_iterator;
+
+         static unsigned int num_rows() { return Rows; }
+         static unsigned int num_cols() { return Cols; }
+
+         Elem       * operator[](unsigned int row)        { return _m[row]; }
+         Elem const * operator[](unsigned int row) const  { return _m[row]; }
+
+         Elem& operator()(unsigned int row, unsigned int col)       { return _m[row-1][col-1]; }
+         Elem  operator()(unsigned int row, unsigned int col) const { return _m[row-1][col-1]; }
+
+         template <typename Vec>
+         void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const
+         {
+            for (unsigned int c = first; c < last; ++c) dst[c-first] = _m[row][c];
+         }
+
+         template <typename Vec>
+         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
+         {
+            for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col];
+         }
+
+         void newsize(unsigned int rows, unsigned int cols) const
+         {
+            assert(rows == Rows && cols == Cols);
+         }
+
+         const_iterator begin() const { return &_m[0][0]; }
+         iterator       begin()       { return &_m[0][0]; }
+         const_iterator end() const { return &_m[0][0] + Rows*Cols; }
+         iterator       end()       { return &_m[0][0] + Rows*Cols; }
+
+      protected:
+         Elem _m[Rows][Cols];
+   };
+
+   template <typename Elem>
+   struct MatrixBase
+   {
+         typedef Elem value_type;
+         typedef Elem element_type;
+
+         typedef Elem const * const_iterator;
+         typedef Elem       * iterator;
+
+         MatrixBase()
+            : _rows(0), _cols(0), _ownsData(true), _m(0)
+         { }
+
+         MatrixBase(unsigned int rows, unsigned int cols)
+            : _rows(rows), _cols(cols), _ownsData(true), _m(0)
+         {
+            if (_rows * _cols == 0) return;
+            _m = new Elem[rows*cols];
+         }
+
+         MatrixBase(unsigned int rows, unsigned int cols, Elem * values)
+            : _rows(rows), _cols(cols), _ownsData(false), _m(values)
+         { }
+
+         MatrixBase(MatrixBase<Elem> const& a)
+            : _ownsData(true), _m(0)
+         {
+            _rows = a._rows; _cols = a._cols;
+            if (_rows * _cols == 0) return;
+            _m = new Elem[_rows*_cols];
+            std::copy(a._m, a._m+_rows*_cols, _m);
+         }
+
+         ~MatrixBase()
+         {
+            if (_ownsData && _m != 0) delete [] _m;
+         }
+
+         MatrixBase& operator=(MatrixBase<Elem> const& a)
+         {
+            if (this == &a) return *this;
+
+            this->newsize(a.num_rows(), a.num_cols());
+
+            std::copy(a._m, a._m+_rows*_cols, _m);
+            return *this;
+         }
+
+         void newsize(unsigned int rows, unsigned int cols)
+         {
+            if (rows == _rows && cols == _cols) return;
+
+            assert(_ownsData);
+
+            __destroy();
+
+            _rows = rows;
+            _cols = cols;
+            if (_rows * _cols == 0) return;
+            _m = new Elem[rows*cols];
+         }
+
+         unsigned int num_rows() const { return _rows; }
+         unsigned int num_cols() const { return _cols; }
+
+         Elem       * operator[](unsigned int row)        { return _m + row*_cols; }
+         Elem const * operator[](unsigned int row) const  { return _m + row*_cols; }
+
+         Elem& operator()(unsigned int row, unsigned int col)       { return _m[(row-1)*_cols + col-1]; }
+         Elem  operator()(unsigned int row, unsigned int col) const { return _m[(row-1)*_cols + col-1]; }
+
+         const_iterator begin() const { return _m; }
+         iterator       begin()       { return _m; }
+         const_iterator end() const { return _m + _rows*_cols; }
+         iterator       end()       { return _m + _rows*_cols; }
+
+         template <typename Vec>
+         void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const
+         {
+            Elem const * v = (*this)[row];
+            for (unsigned int c = first; c < last; ++c) dst[c-first] = v[c];
+         }
+
+         template <typename Vec>
+         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
+         {
+            for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col];
+         }
+
+      protected:
+         void __destroy()
+         {
+            assert(_ownsData);
+            if (_m != 0) delete [] _m;
+            _m = 0;
+            _rows = _cols = 0;
+         }
+
+         unsigned int   _rows, _cols;
+         bool           _ownsData;
+         Elem         * _m;
+   };
+
+   template <typename T>
+   struct CCS_Matrix
+   {
+         CCS_Matrix()
+            : _rows(0), _cols(0)
+         { }
+
+         CCS_Matrix(int const rows, int const cols, vector<pair<int, int> > const& nonZeros)
+            : _rows(rows), _cols(cols)
+         {
+            this->initialize(nonZeros);
+         }
+
+         CCS_Matrix(CCS_Matrix const& b)
+            : _rows(b._rows), _cols(b._cols),
+              _colStarts(b._colStarts), _rowIdxs(b._rowIdxs), _destIdxs(b._destIdxs), _values(b._values)
+         { }
+
+         CCS_Matrix& operator=(CCS_Matrix const& b)
+         {
+            if (this == &b) return *this;
+            _rows       = b._rows;
+            _cols       = b._cols;
+            _colStarts  = b._colStarts;
+            _rowIdxs    = b._rowIdxs;
+            _destIdxs   = b._destIdxs;
+            _values     = b._values;
+            return *this;
+         }
+
+         void create(int const rows, int const cols, vector<pair<int, int> > const& nonZeros)
+         {
+            _rows = rows;
+            _cols = cols;
+            this->initialize(nonZeros);
+         }
+
+         unsigned int num_rows() const { return _rows; }
+         unsigned int num_cols() const { return _cols; }
+
+         int getNonzeroCount() const { return _values.size(); }
+
+         T const * getValues() const { return &_values[0]; }
+         T       * getValues()       { return &_values[0]; }
+
+         int const * getDestIndices()  const { return &_destIdxs[0]; }
+         int const * getColumnStarts() const { return &_colStarts[0]; }
+         int const * getRowIndices()   const { return &_rowIdxs[0]; }
+
+         void getRowRange(unsigned int col, unsigned int& firstRow, unsigned int& lastRow) const
+         {
+            firstRow = _rowIdxs[_colStarts[col]];
+            lastRow  = _rowIdxs[_colStarts[col+1]-1]+1;
+         }
+
+         template <typename Vec>
+         void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const
+         {
+            unsigned int const last = first + len;
+
+            for (int r = 0; r < len; ++r) dst[r] = 0; // Fill vector with zeros
+
+            int const colStart = _colStarts[col];
+            int const colEnd   = _colStarts[col+1];
+
+            int i = colStart;
+            int r;
+            // Skip rows less than the given start row
+            while (i < colEnd && (r = _rowIdxs[i]) < first) ++i;
+
+            // Copy elements until the final row
+            while (i < colEnd && (r = _rowIdxs[i]) < last)
+            {
+               dst[r-first] = _values[i];
+               ++i;
+            }
+         } // end getColumnSlice()
+
+         int getColumnNonzeroCount(unsigned int col) const
+         {
+            int const colStart = _colStarts[col];
+            int const colEnd   = _colStarts[col+1];
+            return colEnd - colStart;
+         }
+
+         template <typename VecA, typename VecB>
+         void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const
+         {
+            int const colStart = _colStarts[col];
+            int const colEnd   = _colStarts[col+1];
+            int const nnz      = colEnd - colStart;
+
+            for (int i = 0; i < nnz; ++i)
+            {
+               rows[i] = _rowIdxs[colStart + i];
+               values[i] = _values[colStart + i];
+            }
+         }
+
+      protected:
+         struct NonzeroInfo
+         {
+               int row, col, serial;
+
+               // Sort wrt the column first
+               bool operator<(NonzeroInfo const& rhs) const
+               {
+                  if (col < rhs.col) return true;
+                  if (col > rhs.col) return false;
+                  return row < rhs.row;
+               }
+         };
+
+         void initialize(std::vector<std::pair<int, int> > const& nonZeros)
+         {
+            using namespace std;
+
+            int const nnz = nonZeros.size();
+
+            _colStarts.resize(_cols + 1);
+            _rowIdxs.resize(nnz);
+
+            vector<NonzeroInfo> nz(nnz);
+            for (int k = 0; k < nnz; ++k)
+            {
+               nz[k].row    = nonZeros[k].first;
+               nz[k].col    = nonZeros[k].second;
+               nz[k].serial = k;
+            }
+
+            // Sort in column major order
+            std::sort(nz.begin(), nz.end());
+
+            for (size_t k = 0; k < nnz; ++k) _rowIdxs[k] = nz[k].row;
+
+            int curCol = -1;
+            for (int k = 0; k < nnz; ++k)
+            {
+               NonzeroInfo const& el = nz[k];
+               if (el.col != curCol)
+               {
+                  // Update empty cols between
+                  for (int c = curCol+1; c < el.col; ++c) _colStarts[c] = k;
+
+                  curCol = el.col;
+                  _colStarts[curCol] = k;
+               } // end if
+            } // end for (k)
+
+            // Update remaining columns
+            for (int c = curCol+1; c <= _cols; ++c) _colStarts[c] = nnz;
+
+            _destIdxs.resize(nnz);
+            for (int k = 0; k < nnz; ++k) _destIdxs[nz[k].serial] = k;
+
+            _values.resize(nnz);
+         } // end initialize()
+
+         int              _rows, _cols;
+         std::vector<int> _colStarts;
+         std::vector<int> _rowIdxs;
+         std::vector<int> _destIdxs;
+         std::vector<T>   _values;
+   }; // end struct CCS_Matrix
+
+//----------------------------------------------------------------------
+
+   template <typename Vec, typename Elem>
+   inline void
+   fillVector(Vec& v, Elem val)
+   {
+      // We do not use std::fill since we rely only on size() and operator[] member functions.
+      for (unsigned int i = 0; i < v.size(); ++i) v[i] = val;
+   }
+
+   template <typename Vec>
+   inline void
+   makeZeroVector(Vec& v)
+   {
+      fillVector(v, 0);
+   }
+
+   template <typename VecA, typename VecB>
+   inline void
+   copyVector(VecA const& src, VecB& dst)
+   {
+      assert(src.size() == dst.size());
+      // We do not use std::fill since we rely only on size() and operator[] member functions.
+      for (unsigned int i = 0; i < src.size(); ++i) dst[i] = src[i];
+   }
+
+   template <typename VecA, typename VecB>
+   inline void
+   copyVectorSlice(VecA const& src, unsigned int srcStart, unsigned int srcLen,
+                   VecB& dst, unsigned int dstStart)
+   {
+      unsigned int const end = std::min(srcStart + srcLen, src.size());
+      unsigned int const sz = dst.size();
+      unsigned int i0, i1;
+      for (i0 = srcStart, i1 = dstStart; i0 < end && i1 < sz; ++i0, ++i1) dst[i1] = src[i0];
+   }
+
+   template <typename Vec>
+   inline typename Vec::value_type
+   norm_L1(Vec const& v)
+   {
+      typename Vec::value_type res(0);
+      for (unsigned int i = 0; i < v.size(); ++i) res += fabs(v[i]);
+      return res;
+   }
+
+   template <typename Vec>
+   inline typename Vec::value_type
+   norm_Linf(Vec const& v)
+   {
+      typename Vec::value_type res(0);
+      for (unsigned int i = 0; i < v.size(); ++i) res = std::max(res, fabs(v[i]));
+      return res;
+   }
+
+   template <typename Vec>
+   inline typename Vec::value_type
+   norm_L2(Vec const& v)
+   {
+      typename Vec::value_type res(0);
+      for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i];
+      return sqrt((double)res);
+   }
+
+   template <typename Vec>
+   inline typename Vec::value_type
+   sqrNorm_L2(Vec const& v)
+   {
+      typename Vec::value_type res(0);
+      for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i];
+      return res;
+   }
+
+   template <typename Vec>
+   inline void
+   normalizeVector(Vec& v)
+   {
+      typename Vec::value_type norm(norm_L2(v));
+      for (unsigned int i = 0; i < v.size(); ++i) v[i] /= norm;
+   }
+
+   template<typename VecA, typename VecB>
+   inline typename VecA::value_type
+   innerProduct(VecA const& a, VecB const& b)
+   {
+      assert(a.size() == b.size());
+      typename VecA::value_type res(0);
+      for (unsigned int i = 0; i < a.size(); ++i) res += a[i] * b[i];
+      return res;
+   }
+
+   template <typename Elem, typename VecA, typename VecB>
+   inline void
+   scaleVector(Elem s, VecA const& v, VecB& dst)
+   {
+      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = s * v[i];
+   }
+
+   template <typename Elem, typename Vec>
+   inline void
+   scaleVectorIP(Elem s, Vec& v)
+   {
+      typedef typename Vec::value_type Elem2;
+      for (unsigned int i = 0; i < v.size(); ++i)
+         v[i] = (Elem2)(v[i] * s);
+   }
+
+   template <typename VecA, typename VecB, typename VecC>
+   inline void
+   makeCrossProductVector(VecA const& v, VecB const& w, VecC& dst)
+   {
+      assert(v.size() == 3);
+      assert(w.size() == 3);
+      assert(dst.size() == 3);
+      dst[0] = v[1]*w[2] - v[2]*w[1];
+      dst[1] = v[2]*w[0] - v[0]*w[2];
+      dst[2] = v[0]*w[1] - v[1]*w[0];
+   }
+
+   template <typename VecA, typename VecB, typename VecC>
+   inline void
+   addVectors(VecA const& v, VecB const& w, VecC& dst)
+   {
+      assert(v.size() == w.size());
+      assert(v.size() == dst.size());
+      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] + w[i];
+   }
+
+   template <typename VecA, typename VecB, typename VecC>
+   inline void
+   subtractVectors(VecA const& v, VecB const& w, VecC& dst)
+   {
+      assert(v.size() == w.size());
+      assert(v.size() == dst.size());
+      for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] - w[i];
+   }
+
+   template <typename MatA, typename MatB>
+   inline void
+   copyMatrix(MatA const& src, MatB& dst)
+   {
+      unsigned int const rows = src.num_rows();
+      unsigned int const cols = src.num_cols();
+      assert(dst.num_rows() == rows);
+      assert(dst.num_cols() == cols);
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) dst[r][c] = src[r][c];
+   }
+
+   template <typename MatA, typename MatB>
+   inline void
+   copyMatrixSlice(MatA const& src, unsigned int rowStart, unsigned int colStart, unsigned int rowLen, unsigned int colLen,
+                   MatB& dst, unsigned int dstRow, unsigned int dstCol)
+   {
+      unsigned int const rows = dst.num_rows();
+      unsigned int const cols = dst.num_cols();
+
+      unsigned int const rowEnd = std::min(rowStart + rowLen, src.num_rows());
+      unsigned int const colEnd = std::min(colStart + colLen, src.num_cols());
+
+      unsigned int c0, c1, r0, r1;
+
+      for (c0 = colStart, c1 = dstCol; c0 < colEnd && c1 < cols; ++c0, ++c1)
+         for (r0 = rowStart, r1 = dstRow; r0 < rowEnd && r1 < rows; ++r0, ++r1)
+            dst[r1][c1] = src[r0][c0];
+   }
+
+   template <typename MatA, typename MatB>
+   inline void
+   makeTransposedMatrix(MatA const& src, MatB& dst)
+   {
+      unsigned int const rows = src.num_rows();
+      unsigned int const cols = src.num_cols();
+      assert(dst.num_cols() == rows);
+      assert(dst.num_rows() == cols);
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) dst[c][r] = src[r][c];
+   }
+
+   template <typename Mat>
+   inline void
+   fillMatrix(Mat& m, typename Mat::value_type val)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) m[r][c] = val;
+   }
+
+   template <typename Mat>
+   inline void
+   makeZeroMatrix(Mat& m)
+   {
+      fillMatrix(m, 0);
+   }
+
+   template <typename Mat>
+   inline void
+   makeIdentityMatrix(Mat& m)
+   {
+      makeZeroMatrix(m);
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      unsigned int n = std::min(rows, cols);
+      for (unsigned int i = 0; i < n; ++i)
+         m[i][i] = 1;
+   }
+
+   template <typename Mat, typename Vec>
+   inline void
+   makeCrossProductMatrix(Vec const& v, Mat& m)
+   {
+      assert(v.size() == 3);
+      assert(m.num_rows() == 3);
+      assert(m.num_cols() == 3);
+      m[0][0] = 0;     m[0][1] = -v[2]; m[0][2] = v[1];
+      m[1][0] = v[2];  m[1][1] = 0;     m[1][2] = -v[0];
+      m[2][0] = -v[1]; m[2][1] = v[0];  m[2][2] = 0;
+   }
+
+   template <typename Mat, typename Vec>
+   inline void
+   makeOuterProductMatrix(Vec const& v, Mat& m)
+   {
+      assert(m.num_cols() == m.num_rows());
+      assert(v.size() == m.num_cols());
+      unsigned const sz = v.size();
+      for (unsigned r = 0; r < sz; ++r)
+         for (unsigned c = 0; c < sz; ++c) m[r][c] = v[r]*v[c];
+   }
+
+   template <typename Mat, typename VecA, typename VecB>
+   inline void
+   makeOuterProductMatrix(VecA const& u, VecB const& v, Mat& m)
+   {
+      assert(m.num_cols() == m.num_rows());
+      assert(u.size() == m.num_cols());
+      assert(v.size() == m.num_cols());
+      unsigned const sz = u.size();
+      for (unsigned r = 0; r < sz; ++r)
+         for (unsigned c = 0; c < sz; ++c) m[r][c] = u[r]*v[c];
+   }
+
+   template <typename MatA, typename MatB, typename MatC>
+   void addMatrices(MatA const& a, MatB const& b, MatC& dst)
+   {
+      assert(a.num_cols() == b.num_cols());
+      assert(a.num_rows() == b.num_rows());
+      assert(dst.num_cols() == a.num_cols());
+      assert(dst.num_rows() == a.num_rows());
+
+      unsigned int const rows = a.num_rows();
+      unsigned int const cols = a.num_cols();
+
+      for (unsigned r = 0; r < rows; ++r)
+         for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] + b[r][c];
+   }
+
+   template <typename MatA, typename MatB>
+   void addMatricesIP(MatA const& a, MatB& dst)
+   {
+      assert(dst.num_cols() == a.num_cols());
+      assert(dst.num_rows() == a.num_rows());
+
+      unsigned int const rows = a.num_rows();
+      unsigned int const cols = a.num_cols();
+
+      for (unsigned r = 0; r < rows; ++r)
+         for (unsigned c = 0; c < cols; ++c) dst[r][c] += a[r][c];
+   }
+
+   template <typename MatA, typename MatB, typename MatC>
+   void subtractMatrices(MatA const& a, MatB const& b, MatC& dst)
+   {
+      assert(a.num_cols() == b.num_cols());
+      assert(a.num_rows() == b.num_rows());
+      assert(dst.num_cols() == a.num_cols());
+      assert(dst.num_rows() == a.num_rows());
+
+      unsigned int const rows = a.num_rows();
+      unsigned int const cols = a.num_cols();
+
+      for (unsigned r = 0; r < rows; ++r)
+         for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] - b[r][c];
+   }
+
+   template <typename MatA, typename Elem, typename MatB>
+   inline void
+   makeScaledMatrix(MatA const& m, Elem scale, MatB& dst)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) dst[r][c] = m[r][c] * scale;
+   }
+
+   template <typename Mat, typename Elem>
+   inline void
+   scaleMatrixIP(Elem scale, Mat& m)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) m[r][c] *= scale;
+   }
+
+   template <typename Mat, typename VecA, typename VecB>
+   inline void
+   multiply_A_v(Mat const& m, VecA const& in, VecB& dst)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      assert(in.size() == cols);
+      assert(dst.size() == rows);
+
+      makeZeroVector(dst);
+
+      for (unsigned int r = 0; r < rows; ++r)
+         for (unsigned int c = 0; c < cols; ++c) dst[r] += m[r][c] * in[c];
+   }
+
+   template <typename Mat, typename VecA, typename VecB>
+   inline void
+   multiply_A_v_projective(Mat const& m, VecA const& in, VecB& dst)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      assert(in.size() == cols-1);
+      assert(dst.size() == rows-1);
+
+      typename VecB::value_type w = m(rows-1, cols-1);
+      unsigned int r, i;
+      for (i = 0; i < cols-1; ++i) w += m(rows-1, i) * in[i];
+      for (r = 0; r < rows-1; ++r) dst[r] = m(r, cols-1);
+      for (r = 0; r < rows-1; ++r)
+         for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c];
+      for (i = 0; i < rows-1; ++i) dst[i] /= w;
+   }
+
+   template <typename Mat, typename VecA, typename VecB>
+   inline void
+   multiply_A_v_affine(Mat const& m, VecA const& in, VecB& dst)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      assert(in.size() == cols-1);
+      assert(dst.size() == rows);
+
+      unsigned int r;
+
+      for (r = 0; r < rows; ++r) dst[r] = m(r, cols-1);
+      for (r = 0; r < rows; ++r)
+         for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c];
+   }
+
+   template <typename Mat, typename VecA, typename VecB>
+   inline void
+   multiply_At_v(Mat const& m, VecA const& in, VecB& dst)
+   {
+      unsigned int const rows = m.num_rows();
+      unsigned int const cols = m.num_cols();
+      assert(in.size() == rows);
+      assert(dst.size() == cols);
+
+      makeZeroVector(dst);
+      for (unsigned int c = 0; c < cols; ++c)
+         for (unsigned int r = 0; r < rows; ++r) dst[c] += m[r][c] * in[r];
+   }
+
+   template <typename MatA, typename MatB>
+   inline void
+   multiply_At_A(MatA const& a, MatB& dst)
+   {
+      assert(dst.num_rows() == a.num_cols());
+      assert(dst.num_cols() == a.num_cols());
+
+      typedef typename MatB::value_type Elem;
+
+      Elem accum;
+      for (unsigned int r = 0; r < a.num_cols(); ++r)
+         for (unsigned int c = 0; c < a.num_cols(); ++c)
+         {
+            accum = 0;
+            for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * a[k][c];
+            dst[r][c] = accum;
+         }
+   }
+
+   template <typename MatA, typename MatB, typename MatC>
+   inline void
+   multiply_A_B(MatA const& a, MatB const& b, MatC& dst)
+   {
+      assert(a.num_cols() == b.num_rows());
+      assert(dst.num_rows() == a.num_rows());
+      assert(dst.num_cols() == b.num_cols());
+
+      typedef typename MatC::value_type Elem;
+
+      Elem accum;
+      for (unsigned int r = 0; r < a.num_rows(); ++r)
+         for (unsigned int c = 0; c < b.num_cols(); ++c)
+         {
+            accum = 0;
+            for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[k][c];
+            dst[r][c] = accum;
+         }
+   }
+
+   template <typename MatA, typename MatB, typename MatC>
+   inline void
+   multiply_At_B(MatA const& a, MatB const& b, MatC& dst)
+   {
+      assert(a.num_rows() == b.num_rows());
+      assert(dst.num_rows() == a.num_cols());
+      assert(dst.num_cols() == b.num_cols());
+
+      typedef typename MatC::value_type Elem;
+
+      Elem accum;
+      for (unsigned int r = 0; r < a.num_cols(); ++r)
+         for (unsigned int c = 0; c < b.num_cols(); ++c)
+         {
+            accum = 0;
+            for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * b[k][c];
+            dst[r][c] = accum;
+         }
+   }
+
+   template <typename MatA, typename MatB, typename MatC>
+   inline void
+   multiply_A_Bt(MatA const& a, MatB const& b, MatC& dst)
+   {
+      assert(a.num_cols() == b.num_cols());
+      assert(dst.num_rows() == a.num_rows());
+      assert(dst.num_cols() == b.num_rows());
+
+      typedef typename MatC::value_type Elem;
+
+      Elem accum;
+      for (unsigned int r = 0; r < a.num_rows(); ++r)
+         for (unsigned int c = 0; c < b.num_rows(); ++c)
+         {
+            accum = 0;
+            for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[c][k];
+            dst[r][c] = accum;
+         }
+   }
+
+   template <typename Mat>
+   inline void
+   transposeMatrixIP(Mat& a)
+   {
+      assert(a.num_rows() == a.num_cols());
+
+      for (unsigned int r = 0; r < a.num_rows(); ++r)
+         for (unsigned int c = 0; c < r; ++c)
+            std::swap(a[r][c], a[c][r]);
+   }
+
+} // end namespace V3D
+
+#endif
diff --git a/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h b/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h
new file mode 100644
index 00000000000..969ec99694f
--- /dev/null
+++ b/extern/libmv/third_party/ssba/Math/v3d_linear_utils.h
@@ -0,0 +1,391 @@
+// -*- C++ -*-
+/*
+Copyright (c) 2008 University of North Carolina at Chapel Hill
+
+This file is part of SSBA (Simple Sparse Bundle Adjustment).
+
+SSBA is free software: you can redistribute it and/or modify it under the
+terms of the GNU Lesser General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option) any
+later version.
+
+SSBA 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 Lesser General Public License for more
+details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with SSBA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef V3D_LINEAR_UTILS_H
+#define V3D_LINEAR_UTILS_H
+
+#include "Math/v3d_linear.h"
+
+#include <iostream>
+
+namespace V3D
+{
+
+   template <typename Elem, int Size>
+   struct InlineVector : public InlineVectorBase<Elem, Size>
+   {
+   }; // end struct InlineVector
+
+   template <typename Elem>
+   struct Vector : public VectorBase<Elem>
+   {
+         Vector()
+            : VectorBase<Elem>()
+         { }
+
+         Vector(unsigned int size)
+            : VectorBase<Elem>(size)
+         { }
+
+         Vector(unsigned int size, Elem * values)
+            : VectorBase<Elem>(size, values)
+         { }
+
+         Vector(Vector<Elem> const& a)
+            : VectorBase<Elem>(a)
+         { }
+
+         Vector<Elem>& operator=(Vector<Elem> const& a)
+         {
+            (VectorBase<Elem>::operator=)(a);
+            return *this;
+         }
+
+         Vector<Elem>& operator+=(Vector<Elem> const& rhs)
+         {
+            addVectorsIP(rhs, *this);
+            return *this;
+         }
+
+         Vector<Elem>& operator*=(Elem scale)
+         {
+            scaleVectorsIP(scale, *this);
+            return *this;
+         }
+
+         Vector<Elem> operator+(Vector<Elem> const& rhs) const
+         {
+            Vector<Elem> res(this->size());
+            addVectors(*this, rhs, res);
+            return res;
+         }
+
+         Vector<Elem> operator-(Vector<Elem> const& rhs) const
+         {
+            Vector<Elem> res(this->size());
+            subtractVectors(*this, rhs, res);
+            return res;
+         }
+
+         Elem operator*(Vector<Elem> const& rhs) const
+         {
+            return innerProduct(*this, rhs);
+         }
+
+   }; // end struct Vector
+
+   template <typename Elem, int Rows, int Cols>
+   struct InlineMatrix : public InlineMatrixBase<Elem, Rows, Cols>
+   {
+   }; // end struct InlineMatrix
+
+   template <typename Elem>
+   struct Matrix : public MatrixBase<Elem>
+   {
+         Matrix()
+            : MatrixBase<Elem>()
+         { }
+
+         Matrix(unsigned int rows, unsigned int cols)
+            : MatrixBase<Elem>(rows, cols)
+         { }
+
+         Matrix(unsigned int rows, unsigned int cols, Elem * values)
+            : MatrixBase<Elem>(rows, cols, values)
+         { }
+
+         Matrix(Matrix<Elem> const& a)
+            : MatrixBase<Elem>(a)
+         { }
+
+         Matrix<Elem>& operator=(Matrix<Elem> const& a)
+         {
+            (MatrixBase<Elem>::operator=)(a);
+            return *this;
+         }
+
+         Matrix<Elem>& operator+=(Matrix<Elem> const& rhs)
+         {
+            addMatricesIP(rhs, *this);
+            return *this;
+         }
+
+         Matrix<Elem>& operator*=(Elem scale)
+         {
+            scaleMatrixIP(scale, *this);
+            return *this;
+         }
+
+         Matrix<Elem> operator+(Matrix<Elem> const& rhs) const
+         {
+            Matrix<Elem> res(this->num_rows(), this->num_cols());
+            addMatrices(*this, rhs, res);
+            return res;
+         }
+
+         Matrix<Elem> operator-(Matrix<Elem> const& rhs) const
+         {
+            Matrix<Elem> res(this->num_rows(), this->num_cols());
+            subtractMatrices(*this, rhs, res);
+            return res;
+         }
+
+   }; // end struct Matrix
+
+//----------------------------------------------------------------------
+
+   typedef InlineVector<float, 2>  Vector2f;
+   typedef InlineVector<double, 2> Vector2d;
+   typedef InlineVector<float, 3>  Vector3f;
+   typedef InlineVector<double, 3> Vector3d;
+   typedef InlineVector<float, 4>  Vector4f;
+   typedef InlineVector<double, 4> Vector4d;
+
+   typedef InlineMatrix<float, 2, 2>  Matrix2x2f;
+   typedef InlineMatrix<double, 2, 2> Matrix2x2d;
+   typedef InlineMatrix<float, 3, 3>  Matrix3x3f;
+   typedef InlineMatrix<double, 3, 3> Matrix3x3d;
+   typedef InlineMatrix<float, 4, 4>  Matrix4x4f;
+   typedef InlineMatrix<double, 4, 4> Matrix4x4d;
+
+   typedef InlineMatrix<float, 2, 3>  Matrix2x3f;
+   typedef InlineMatrix<double, 2, 3> Matrix2x3d;
+   typedef InlineMatrix<float, 3, 4>  Matrix3x4f;
+   typedef InlineMatrix<double, 3, 4> Matrix3x4d;
+
+   template <typename Elem>
+   struct VectorArray
+   {
+         VectorArray(unsigned count, unsigned size)
+            : _count(count), _size(size), _values(0), _vectors(0)
+         {
+            unsigned const nTotal = _count * _size;
+            if (count > 0) _vectors = new Vector<Elem>[count];
+            if (nTotal > 0) _values = new Elem[nTotal];
+            for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector<Elem>(_size, _values + i*_size);
+         }
+
+         VectorArray(unsigned count, unsigned size, Elem initVal)
+            : _count(count), _size(size), _values(0), _vectors(0)
+         {
+            unsigned const nTotal = _count * _size;
+            if (count > 0) _vectors = new Vector<Elem>[count];
+            if (nTotal > 0) _values = new Elem[nTotal];
+            for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector<Elem>(_size, _values + i*_size);
+            std::fill(_values, _values + nTotal, initVal);
+         }
+
+         ~VectorArray()
+         {
+            delete [] _values;
+            delete [] _vectors;
+         }
+
+         unsigned count() const { return _count; }
+         unsigned size()  const { return _size; }
+
+         //! Get the submatrix at position ix
+         Vector<Elem> const& operator[](unsigned ix) const
+         {
+            return _vectors[ix];
+         }
+
+         //! Get the submatrix at position ix
+         Vector<Elem>& operator[](unsigned ix)
+         {
+            return _vectors[ix];
+         }
+
+      protected:
+         unsigned       _count, _size;
+         Elem         * _values;
+         Vector<Elem> * _vectors;
+
+      private:
+         VectorArray(VectorArray const&);
+         void operator=(VectorArray const&);
+   };
+
+   template <typename Elem>
+   struct MatrixArray
+   {
+         MatrixArray(unsigned count, unsigned nRows, unsigned nCols)
+            : _count(count), _rows(nRows), _columns(nCols), _values(0), _matrices(0)
+         {
+            unsigned const nTotal = _count * _rows * _columns;
+            if (count > 0) _matrices = new Matrix<Elem>[count];
+            if (nTotal > 0) _values = new double[nTotal];
+            for (unsigned i = 0; i < _count; ++i)
+               new (&_matrices[i]) Matrix<Elem>(_rows, _columns, _values + i*(_rows*_columns));
+         }
+
+         ~MatrixArray()
+         {
+            delete [] _matrices;
+            delete [] _values;
+         }
+
+         //! Get the submatrix at position ix
+         Matrix<Elem> const& operator[](unsigned ix) const
+         {
+            return _matrices[ix];
+         }
+
+         //! Get the submatrix at position ix
+         Matrix<Elem>& operator[](unsigned ix)
+         {
+            return _matrices[ix];
+         }
+
+         unsigned count()    const { return _count; }
+         unsigned num_rows() const { return _rows; }
+         unsigned num_cols() const { return _columns; }
+
+      protected:
+         unsigned       _count, _rows, _columns;
+         double       * _values;
+         Matrix<Elem> * _matrices;
+
+      private:
+         MatrixArray(MatrixArray const&);
+         void operator=(MatrixArray const&);
+   };
+
+//----------------------------------------------------------------------
+
+   template <typename Elem, int Size>
+   inline InlineVector<Elem, Size>
+   operator+(InlineVector<Elem, Size> const& v, InlineVector<Elem, Size> const& w)
+   {
+      InlineVector<Elem, Size> res;
+      addVectors(v, w, res);
+      return res;
+   }
+
+   template <typename Elem, int Size>
+   inline InlineVector<Elem, Size>
+   operator-(InlineVector<Elem, Size> const& v, InlineVector<Elem, Size> const& w)
+   {
+      InlineVector<Elem, Size> res;
+      subtractVectors(v, w, res);
+      return res;
+   }
+
+   template <typename Elem, int Size>
+   inline InlineVector<Elem, Size>
+   operator*(Elem scale, InlineVector<Elem, Size> const& v)
+   {
+      InlineVector<Elem, Size> res;
+      scaleVector(scale, v, res);
+      return res;
+   }
+
+   template <typename Elem, int Rows, int Cols>
+   inline InlineVector<Elem, Rows>
+   operator*(InlineMatrix<Elem, Rows, Cols> const& A, InlineVector<Elem, Cols> const& v)
+   {
+      InlineVector<Elem, Rows> res;
+      multiply_A_v(A, v, res);
+      return res;
+   }
+
+   template <typename Elem, int RowsA, int ColsA, int ColsB>
+   inline InlineMatrix<Elem, RowsA, ColsB>
+   operator*(InlineMatrix<Elem, RowsA, ColsA> const& A, InlineMatrix<Elem, ColsA, ColsB> const& B)
+   {
+      InlineMatrix<Elem, RowsA, ColsB> res;
+      multiply_A_B(A, B, res);
+      return res;
+   }
+
+   template <typename Elem, int Rows, int Cols>
+   inline InlineMatrix<Elem, Cols, Rows>
+   transposedMatrix(InlineMatrix<Elem, Rows, Cols> const& A)
+   {
+      InlineMatrix<Elem, Cols, Rows> At;
+      makeTransposedMatrix(A, At);
+      return At;
+   }
+
+   template <typename Elem>
+   inline InlineMatrix<Elem, 3, 3>
+   invertedMatrix(InlineMatrix<Elem, 3, 3> const& A)
+   {
+      Elem a = A[0][0], b = A[0][1], c = A[0][2];
+      Elem d = A[1][0], e = A[1][1], f = A[1][2];
+      Elem g = A[2][0], h = A[2][1], i = A[2][2];
+
+      Elem const det = a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h;
+
+      InlineMatrix<Elem, 3, 3> res;
+      res[0][0] = e*i-f*h; res[0][1] = c*h-b*i; res[0][2] = b*f-c*e;
+      res[1][0] = f*g-d*i; res[1][1] = a*i-c*g; res[1][2] = c*d-a*f;
+      res[2][0] = d*h-e*g; res[2][1] = b*g-a*h; res[2][2] = a*e-b*d;
+
+      scaleMatrixIP(1.0/det, res);
+      return res;
+   }
+
+   template <typename Elem>
+   inline InlineVector<Elem, 2>
+   makeVector2(Elem a, Elem b)
+   {
+      InlineVector<Elem, 2> res;
+      res[0] = a; res[1] = b;
+      return res;
+   }
+
+   template <typename Elem>
+   inline InlineVector<Elem, 3>
+   makeVector3(Elem a, Elem b, Elem c)
+   {
+      InlineVector<Elem, 3> res;
+      res[0] = a; res[1] = b; res[2] = c;
+      return res;
+   }
+
+   template <typename Vec>
+   inline void
+   displayVector(Vec const& v)
+   {
+      using namespace std;
+
+      for (int r = 0; r < v.size(); ++r)
+         cout << v[r] << " ";
+      cout << endl;
+   }
+
+   template <typename Mat>
+   inline void
+   displayMatrix(Mat const& A)
+   {
+      using namespace std;
+
+      for (int r = 0; r < A.num_rows(); ++r)
+      {
+         for (int c = 0; c < A.num_cols(); ++c)
+            cout << A[r][c] << " ";
+         cout << endl;
+      }
+   }
+
+} // end namespace V3D
+
+#endif
diff --git a/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h
new file mode 100644
index 00000000000..9e38b92a94b
--- /dev/null
+++ b/extern/libmv/third_party/ssba/Math/v3d_mathutilities.h
@@ -0,0 +1,59 @@
+// -*- C++ -*-
+/*
+Copyright (c) 2008 University of North Carolina at Chapel Hill
+
+This file is part of SSBA (Simple Sparse Bundle Adjustment).
+
+SSBA is free software: you can redistribute it and/or modify it under the
+terms of the GNU Lesser General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option) any
+later version.
+
+SSBA 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 Lesser General Public License for more
+details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with SSBA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef V3D_MATH_UTILITIES_H
+#define V3D_MATH_UTILITIES_H
+
+#include "Math/v3d_linear.h"
+#include "Math/v3d_linear_utils.h"
+
+#include <vector>
+
+namespace V3D
+{
+
+   template <typename Vec, typename Mat>
+   inline void
+   createRotationMatrixRodriguez(Vec const& omega, Mat& R)
+   {
+      assert(omega.size() == 3);
+      assert(R.num_rows() == 3);
+      assert(R.num_cols() == 3);
+
+      double const theta = norm_L2(omega);
+      makeIdentityMatrix(R);
+      if (fabs(theta) > 1e-6)
+      {
+         Matrix3x3d J, J2;
+         makeCrossProductMatrix(omega, J);
+         multiply_A_B(J, J, J2);
+         double const c1 = sin(theta)/theta;
+         double const c2 = (1-cos(theta))/(theta*theta);
+         for (int i = 0; i < 3; ++i)
+            for (int j = 0; j < 3; ++j)
+               R[i][j] += c1*J[i][j] + c2*J2[i][j];
+      }
+   } // end createRotationMatrixRodriguez()
+
+   template <typename T> inline double sqr(T x) { return x*x; }
+
+} // namespace V3D
+
+#endif
diff --git a/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp b/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp
new file mode 100644
index 00000000000..234815fcd1f
--- /dev/null
+++ b/extern/libmv/third_party/ssba/Math/v3d_optimization.cpp
@@ -0,0 +1,955 @@
+/*
+Copyright (c) 2008 University of North Carolina at Chapel Hill
+
+This file is part of SSBA (Simple Sparse Bundle Adjustment).
+
+SSBA is free software: you can redistribute it and/or modify it under the
+terms of the GNU Lesser General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option) any
+later version.
+
+SSBA 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 Lesser General Public License for more
+details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with SSBA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Math/v3d_optimization.h"
+
+#if defined(V3DLIB_ENABLE_SUITESPARSE)
+//# include "COLAMD/Include/colamd.h"
+# include "colamd.h"
+extern "C"
+{
+//# include "LDL/Include/ldl.h"
+# include "ldl.h"
+}
+#endif
+
+#include <iostream>
+#include <map>
+
+#define USE_BLOCK_REORDERING 1
+#define USE_MULTIPLICATIVE_UPDATE 1
+
+using namespace std;
+
+namespace
+{
+
+   using namespace V3D;
+
+   inline double
+   squaredResidual(VectorArray<double> const& e)
+   {
+      int const N = e.count();
+      int const M = e.size();
+
+      double res = 0.0;
+
+      for (int n = 0; n < N; ++n)
+         for (int m = 0; m < M; ++m)
+            res += e[n][m] * e[n][m];
+
+      return res;
+   } // end squaredResidual()
+
+} // end namespace <>
+
+namespace V3D
+{
+
+   int optimizerVerbosenessLevel = 0;
+
+#if defined(V3DLIB_ENABLE_SUITESPARSE)
+
+   void
+   SparseLevenbergOptimizer::setupSparseJtJ()
+   {
+      int const nVaryingA = _nParametersA - _nNonvaryingA;
+      int const nVaryingB = _nParametersB - _nNonvaryingB;
+      int const nVaryingC = _paramDimensionC - _nNonvaryingC;
+
+      int const bColumnStart = nVaryingA*_paramDimensionA;
+      int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB;
+      int const nColumns     = cColumnStart + nVaryingC;
+
+      _jointNonzerosW.clear();
+      _jointIndexW.resize(_nMeasurements);
+#if 1
+      {
+         map<pair<int, int>, int> jointNonzeroMap;
+         for (size_t k = 0; k < _nMeasurements; ++k)
+         {
+            int const i = _correspondingParamA[k] - _nNonvaryingA;
+            int const j = _correspondingParamB[k] - _nNonvaryingB;
+            if (i >= 0 && j >= 0)
+            {
+               map<pair<int, int>, int>::const_iterator p = jointNonzeroMap.find(make_pair(i, j));
+               if (p == jointNonzeroMap.end())
+               {
+                  jointNonzeroMap.insert(make_pair(make_pair(i, j), _jointNonzerosW.size()));
+                  _jointIndexW[k] = _jointNonzerosW.size();
+                  _jointNonzerosW.push_back(make_pair(i, j));
+               }
+               else
+               {
+                  _jointIndexW[k] = (*p).second;
+               } // end if
+            } // end if
+         } // end for (k)
+      } // end scope
+#else
+      for (size_t k = 0; k < _nMeasurements; ++k)
+      {
+         int const i = _correspondingParamA[k] - _nNonvaryingA;
+         int const j = _correspondingParamB[k] - _nNonvaryingB;
+         if (i >= 0 && j >= 0)
+         {
+            _jointIndexW[k] = _jointNonzerosW.size();
+            _jointNonzerosW.push_back(make_pair(i, j));
+         }
+      } // end for (k)
+#endif
+
+#if defined(USE_BLOCK_REORDERING)
+      int const bBlockColumnStart = nVaryingA;
+      int const cBlockColumnStart = bBlockColumnStart + nVaryingB;
+
+      int const nBlockColumns = cBlockColumnStart + ((nVaryingC > 0) ? 1 : 0);
+
+      //cout << "nBlockColumns = " << nBlockColumns << endl;
+
+      // For the column reordering we treat the columns belonging to one set
+      // of parameters as one (logical) column.
+
+      // Determine non-zeros of JtJ (we forget about the non-zero diagonal for now)
+      // Only consider nonzeros of Ai^t * Bj induced by the measurements.
+      vector<pair<int, int> > nz_blockJtJ(_jointNonzerosW.size());
+      for (int k = 0; k < _jointNonzerosW.size(); ++k)
+      {
+         nz_blockJtJ[k].first  = _jointNonzerosW[k].second + bBlockColumnStart;
+         nz_blockJtJ[k].second = _jointNonzerosW[k].first;
+      }
+
+      if (nVaryingC > 0)
+      {
+         // We assume, that the global unknowns are linked to every other variable.
+         for (int i = 0; i < nVaryingA; ++i)
+            nz_blockJtJ.push_back(make_pair(cBlockColumnStart, i));
+         for (int j = 0; j < nVaryingB; ++j)
+            nz_blockJtJ.push_back(make_pair(cBlockColumnStart, j + bBlockColumnStart));
+      } // end if
+
+      int const nnzBlock = nz_blockJtJ.size();
+
+      vector<int> permBlockJtJ(nBlockColumns + 1);
+
+      if (nnzBlock > 0)
+      {
+//          cout << "nnzBlock = " << nnzBlock << endl;
+
+         CCS_Matrix<int> blockJtJ(nBlockColumns, nBlockColumns, nz_blockJtJ);
+
+//          cout << " nz_blockJtJ: " << endl;
+//          for (size_t k = 0; k < nz_blockJtJ.size(); ++k)
+//             cout << " " << nz_blockJtJ[k].first << ":" << nz_blockJtJ[k].second << endl;
+//          cout << endl;
+
+         int * colStarts = (int *)blockJtJ.getColumnStarts();
+         int * rowIdxs   = (int *)blockJtJ.getRowIndices();
+
+//          cout << "blockJtJ_colStarts = ";
+//          for (int k = 0; k <= nBlockColumns; ++k) cout << colStarts[k] << " ";
+//          cout << endl;
+
+//          cout << "blockJtJ_rowIdxs = ";
+//          for (int k = 0; k < nnzBlock; ++k) cout << rowIdxs[k] << " ";
+//          cout << endl;
+
+         int stats[COLAMD_STATS];
+         symamd(nBlockColumns, rowIdxs, colStarts, &permBlockJtJ[0], (double *) NULL, stats, &calloc, &free);
+         if (optimizerVerbosenessLevel >= 2) symamd_report(stats);
+      }
+      else
+      {
+         for (int k = 0; k < permBlockJtJ.size(); ++k) permBlockJtJ[k] = k;
+      } // end if
+
+//       cout << "permBlockJtJ = ";
+//       for (int k = 0; k < permBlockJtJ.size(); ++k)
+//          cout << permBlockJtJ[k] << " ";
+//       cout << endl;
+
+      // From the determined symbolic permutation with logical variables, determine the actual ordering
+      _perm_JtJ.resize(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC + 1);
+
+      int curDstCol = 0;
+      for (int k = 0; k < permBlockJtJ.size()-1; ++k)
+      {
+         int const srcCol = permBlockJtJ[k];
+         if (srcCol < nVaryingA)
+         {
+            for (int n = 0; n < _paramDimensionA; ++n)
+               _perm_JtJ[curDstCol + n] = srcCol*_paramDimensionA + n;
+            curDstCol += _paramDimensionA;
+         }
+         else if (srcCol >= bBlockColumnStart && srcCol < cBlockColumnStart)
+         {
+            int const bStart = nVaryingA*_paramDimensionA;
+            int const j = srcCol - bBlockColumnStart;
+
+            for (int n = 0; n < _paramDimensionB; ++n)
+               _perm_JtJ[curDstCol + n] = bStart + j*_paramDimensionB + n;
+            curDstCol += _paramDimensionB;
+         }
+         else if (srcCol == cBlockColumnStart)
+         {
+            int const cStart = nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB;
+
+            for (int n = 0; n < nVaryingC; ++n)
+               _perm_JtJ[curDstCol + n] = cStart + n;
+            curDstCol += nVaryingC;
+         }
+         else
+         {
+            cerr << "Should not reach " << __LINE__ << ":" << __LINE__ << "!" << endl;
+            assert(false);
+         }
+      }
+#else
+      vector<pair<int, int> > nz, nzL;
+      this->serializeNonZerosJtJ(nz);
+
+      for (int k = 0; k < nz.size(); ++k)
+      {
+         // Swap rows and columns, since serializeNonZerosJtJ() generates the
+         // upper triangular part but symamd wants the lower triangle.
+         nzL.push_back(make_pair(nz[k].second, nz[k].first));
+      }
+
+      _perm_JtJ.resize(nColumns+1);
+
+      if (nzL.size() > 0)
+      {
+         CCS_Matrix<int> symbJtJ(nColumns, nColumns, nzL);
+
+         int * colStarts = (int *)symbJtJ.getColumnStarts();
+         int * rowIdxs   = (int *)symbJtJ.getRowIndices();
+
+//       cout << "symbJtJ_colStarts = ";
+//       for (int k = 0; k <= nColumns; ++k) cout << colStarts[k] << " ";
+//       cout << endl;
+
+//       cout << "symbJtJ_rowIdxs = ";
+//       for (int k = 0; k < nzL.size(); ++k) cout << rowIdxs[k] << " ";
+//       cout << endl;
+
+         int stats[COLAMD_STATS];
+         symamd(nColumns, rowIdxs, colStarts, &_perm_JtJ[0], (double *) NULL, stats, &calloc, &free);
+         if (optimizerVerbosenessLevel >= 2) symamd_report(stats);
+      }
+      else
+      {
+         for (int k = 0; k < _perm_JtJ.size(); ++k) _perm_JtJ[k] = k;
+      } //// end if
+#endif
+      _perm_JtJ.back() = _perm_JtJ.size() - 1;
+
+//       cout << "_perm_JtJ = ";
+//       for (int k = 0; k < _perm_JtJ.size(); ++k) cout << _perm_JtJ[k] << " ";
+//       cout << endl;
+
+      // Finally, compute the inverse of the full permutation.
+      _invPerm_JtJ.resize(_perm_JtJ.size());
+      for (size_t k = 0; k < _perm_JtJ.size(); ++k)
+         _invPerm_JtJ[_perm_JtJ[k]] = k;
+
+      vector<pair<int, int> > nz_JtJ;
+      this->serializeNonZerosJtJ(nz_JtJ);
+
+      for (int k = 0; k < nz_JtJ.size(); ++k)
+      {
+         int const i = nz_JtJ[k].first;
+         int const j = nz_JtJ[k].second;
+
+         int pi = _invPerm_JtJ[i];
+         int pj = _invPerm_JtJ[j];
+         // Swap values if in lower triangular part
+         if (pi > pj) std::swap(pi, pj);
+         nz_JtJ[k].first = pi;
+         nz_JtJ[k].second = pj;
+      }
+
+      int const nnz   = nz_JtJ.size();
+
+//       cout << "nz_JtJ = ";
+//       for (int k = 0; k < nnz; ++k) cout << nz_JtJ[k].first << ":" << nz_JtJ[k].second << " ";
+//       cout << endl;
+
+      _JtJ.create(nColumns, nColumns, nz_JtJ);
+
+//       cout << "_colStart_JtJ = ";
+//       for (int k = 0; k < _JtJ.num_cols(); ++k) cout << _JtJ.getColumnStarts()[k] << " ";
+//       cout << endl;
+
+//       cout << "_rowIdxs_JtJ = ";
+//       for (int k = 0; k < nnz; ++k) cout << _JtJ.getRowIndices()[k] << " ";
+//       cout << endl;
+
+      vector<int> workFlags(nColumns);
+
+      _JtJ_Lp.resize(nColumns+1);
+      _JtJ_Parent.resize(nColumns);
+      _JtJ_Lnz.resize(nColumns);
+
+      ldl_symbolic(nColumns, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(),
+                   &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0],
+                   &workFlags[0], NULL, NULL);
+
+      if (optimizerVerbosenessLevel >= 1)
+         cout << "SparseLevenbergOptimizer: Nonzeros in LDL decomposition: " << _JtJ_Lp[nColumns] << endl;
+
+   } // end SparseLevenbergOptimizer::setupSparseJtJ()
+
+   void
+   SparseLevenbergOptimizer::serializeNonZerosJtJ(vector<pair<int, int> >& dst) const
+   {
+      int const nVaryingA = _nParametersA - _nNonvaryingA;
+      int const nVaryingB = _nParametersB - _nNonvaryingB;
+      int const nVaryingC = _paramDimensionC - _nNonvaryingC;
+
+      int const bColumnStart = nVaryingA*_paramDimensionA;
+      int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB;
+
+      dst.clear();
+
+      // Add the diagonal block matrices (only the upper triangular part).
+
+      // Ui submatrices of JtJ
+      for (int i = 0; i < nVaryingA; ++i)
+      {
+         int const i0 = i * _paramDimensionA;
+
+         for (int c = 0; c < _paramDimensionA; ++c)
+            for (int r = 0; r <= c; ++r)
+               dst.push_back(make_pair(i0 + r, i0 + c));
+      }
+
+      // Vj submatrices of JtJ
+      for (int j = 0; j < nVaryingB; ++j)
+      {
+         int const j0 = j*_paramDimensionB + bColumnStart;
+
+         for (int c = 0; c < _paramDimensionB; ++c)
+            for (int r = 0; r <= c; ++r)
+               dst.push_back(make_pair(j0 + r, j0 + c));
+      }
+
+      // Z submatrix of JtJ
+      for (int c = 0; c < nVaryingC; ++c)
+         for (int r = 0; r <= c; ++r)
+            dst.push_back(make_pair(cColumnStart + r, cColumnStart + c));
+
+      // Add the elements i and j linked by an observation k
+      // W submatrix of JtJ
+      for (size_t n = 0; n < _jointNonzerosW.size(); ++n)
+      {
+         int const i0 = _jointNonzerosW[n].first;
+         int const j0 = _jointNonzerosW[n].second;
+         int const r0 = i0*_paramDimensionA;
+         int const c0 = j0*_paramDimensionB + bColumnStart;
+
+         for (int r = 0; r < _paramDimensionA; ++r)
+            for (int c = 0; c < _paramDimensionB; ++c)
+               dst.push_back(make_pair(r0 + r, c0 + c));
+      } // end for (n)
+
+      if (nVaryingC > 0)
+      {
+         // Finally, add the dense columns linking i (resp. j) with the global parameters.
+         // X submatrix of JtJ
+         for (int i = 0; i < nVaryingA; ++i)
+         {
+            int const i0 = i*_paramDimensionA;
+
+            for (int r = 0; r < _paramDimensionA; ++r)
+               for (int c = 0; c < nVaryingC; ++c)
+                  dst.push_back(make_pair(i0 + r, cColumnStart + c));
+         }
+
+         // Y submatrix of JtJ
+         for (int j = 0; j < nVaryingB; ++j)
+         {
+            int const j0 = j*_paramDimensionB + bColumnStart;
+
+            for (int r = 0; r < _paramDimensionB; ++r)
+               for (int c = 0; c < nVaryingC; ++c)
+                  dst.push_back(make_pair(j0 + r, cColumnStart + c));
+         }
+      } // end if
+   } // end SparseLevenbergOptimizer::serializeNonZerosJtJ()
+
+   void
+   SparseLevenbergOptimizer::fillSparseJtJ(MatrixArray<double> const& Ui,
+                                           MatrixArray<double> const& Vj,
+                                           MatrixArray<double> const& Wn,
+                                           Matrix<double> const& Z,
+                                           Matrix<double> const& X,
+                                           Matrix<double> const& Y)
+   {
+      int const nVaryingA = _nParametersA - _nNonvaryingA;
+      int const nVaryingB = _nParametersB - _nNonvaryingB;
+      int const nVaryingC = _paramDimensionC - _nNonvaryingC;
+
+      int const bColumnStart = nVaryingA*_paramDimensionA;
+      int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB;
+
+      int const nCols = _JtJ.num_cols();
+      int const nnz   = _JtJ.getNonzeroCount();
+
+      // The following has to replicate the procedure as in serializeNonZerosJtJ()
+
+      int serial = 0;
+
+      double * values = _JtJ.getValues();
+      int const * destIdxs = _JtJ.getDestIndices();
+
+      // Add the diagonal block matrices (only the upper triangular part).
+
+      // Ui submatrices of JtJ
+      for (int i = 0; i < nVaryingA; ++i)
+      {
+         int const i0 = i * _paramDimensionA;
+
+         for (int c = 0; c < _paramDimensionA; ++c)
+            for (int r = 0; r <= c; ++r, ++serial)
+               values[destIdxs[serial]] = Ui[i][r][c];
+      }
+
+      // Vj submatrices of JtJ
+      for (int j = 0; j < nVaryingB; ++j)
+      {
+         int const j0 = j*_paramDimensionB + bColumnStart;
+
+         for (int c = 0; c < _paramDimensionB; ++c)
+            for (int r = 0; r <= c; ++r, ++serial)
+               values[destIdxs[serial]] = Vj[j][r][c];
+      }
+
+      // Z submatrix of JtJ
+      for (int c = 0; c < nVaryingC; ++c)
+         for (int r = 0; r <= c; ++r, ++serial)
+            values[destIdxs[serial]] = Z[r][c];
+
+      // Add the elements i and j linked by an observation k
+      // W submatrix of JtJ
+      for (size_t n = 0; n < _jointNonzerosW.size(); ++n)
+      {
+         for (int r = 0; r < _paramDimensionA; ++r)
+            for (int c = 0; c < _paramDimensionB; ++c, ++serial)
+               values[destIdxs[serial]] = Wn[n][r][c];
+      } // end for (k)
+
+      if (nVaryingC > 0)
+      {
+         // Finally, add the dense columns linking i (resp. j) with the global parameters.
+         // X submatrix of JtJ
+         for (int i = 0; i < nVaryingA; ++i)
+         {
+            int const r0 = i * _paramDimensionA;
+            for (int r = 0; r < _paramDimensionA; ++r)
+               for (int c = 0; c < nVaryingC; ++c, ++serial)
+                  values[destIdxs[serial]] = X[r0+r][c];
+         }
+
+         // Y submatrix of JtJ
+         for (int j = 0; j < nVaryingB; ++j)
+         {
+            int const r0 = j * _paramDimensionB;
+            for (int r = 0; r < _paramDimensionB; ++r)
+               for (int c = 0; c < nVaryingC; ++c, ++serial)
+                  values[destIdxs[serial]] = Y[r0+r][c];
+         }
+      } // end if
+   } // end SparseLevenbergOptimizer::fillSparseJtJ()
+
+   void
+   SparseLevenbergOptimizer::minimize()
+   {
+      status = LEVENBERG_OPTIMIZER_TIMEOUT;
+      bool computeDerivatives = true;
+
+      int const nVaryingA = _nParametersA - _nNonvaryingA;
+      int const nVaryingB = _nParametersB - _nNonvaryingB;
+      int const nVaryingC = _paramDimensionC - _nNonvaryingC;
+
+      if (nVaryingA == 0 && nVaryingB == 0 && nVaryingC == 0)
+      {
+         // No degrees of freedom, nothing to optimize.
+         status = LEVENBERG_OPTIMIZER_CONVERGED;
+         return;
+      }
+
+      this->setupSparseJtJ();
+
+      Vector<double> weights(_nMeasurements);
+
+      MatrixArray<double> Ak(_nMeasurements, _measurementDimension, _paramDimensionA);
+      MatrixArray<double> Bk(_nMeasurements, _measurementDimension, _paramDimensionB);
+      MatrixArray<double> Ck(_nMeasurements, _measurementDimension, _paramDimensionC);
+
+      MatrixArray<double> Ui(nVaryingA, _paramDimensionA, _paramDimensionA);
+      MatrixArray<double> Vj(nVaryingB, _paramDimensionB, _paramDimensionB);
+
+      // Wn = Ak^t*Bk
+      MatrixArray<double> Wn(_jointNonzerosW.size(), _paramDimensionA, _paramDimensionB);
+
+      Matrix<double> Z(nVaryingC, nVaryingC);
+
+      // X = A^t*C
+      Matrix<double> X(nVaryingA*_paramDimensionA, nVaryingC);
+      // Y = B^t*C
+      Matrix<double> Y(nVaryingB*_paramDimensionB, nVaryingC);
+
+      VectorArray<double> residuals(_nMeasurements, _measurementDimension);
+      VectorArray<double> residuals2(_nMeasurements, _measurementDimension);
+
+      VectorArray<double> diagUi(nVaryingA, _paramDimensionA);
+      VectorArray<double> diagVj(nVaryingB, _paramDimensionB);
+      Vector<double> diagZ(nVaryingC);
+
+      VectorArray<double> At_e(nVaryingA, _paramDimensionA);
+      VectorArray<double> Bt_e(nVaryingB, _paramDimensionB);
+      Vector<double> Ct_e(nVaryingC);
+
+      Vector<double> Jt_e(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC);
+
+      Vector<double> delta(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC);
+      Vector<double> deltaPerm(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC);
+
+      VectorArray<double> deltaAi(_nParametersA, _paramDimensionA);
+      VectorArray<double> deltaBj(_nParametersB, _paramDimensionB);
+      Vector<double> deltaC(_paramDimensionC);
+
+      double err = 0.0;
+
+      for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration)
+      {
+         if (optimizerVerbosenessLevel >= 2)
+            cout << "SparseLevenbergOptimizer: currentIteration: " << currentIteration << endl;
+         if (computeDerivatives)
+         {
+            this->evalResidual(residuals);
+            this->fillWeights(residuals, weights);
+            for (int k = 0; k < _nMeasurements; ++k)
+               scaleVectorIP(weights[k], residuals[k]);
+
+            err = squaredResidual(residuals);
+
+            if (optimizerVerbosenessLevel >= 1) cout << "SparseLevenbergOptimizer: |residual|^2 = " << err << endl;
+            if (optimizerVerbosenessLevel >= 2) cout << "SparseLevenbergOptimizer: lambda = " << lambda << endl;
+
+            for (int k = 0; k < residuals.count(); ++k) scaleVectorIP(-1.0, residuals[k]);
+
+            this->setupJacobianGathering();
+            this->fillAllJacobians(weights, Ak, Bk, Ck);
+
+            // Compute the different parts of J^t*e
+            if (nVaryingA > 0)
+            {
+               for (int i = 0; i < nVaryingA; ++i) makeZeroVector(At_e[i]);
+
+               Vector<double> tmp(_paramDimensionA);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const i = _correspondingParamA[k] - _nNonvaryingA;
+                  if (i < 0) continue;
+                  multiply_At_v(Ak[k], residuals[k], tmp);
+                  addVectors(tmp, At_e[i], At_e[i]);
+               } // end for (k)
+            } // end if
+
+            if (nVaryingB > 0)
+            {
+               for (int j = 0; j < nVaryingB; ++j) makeZeroVector(Bt_e[j]);
+
+               Vector<double> tmp(_paramDimensionB);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const j = _correspondingParamB[k] - _nNonvaryingB;
+                  if (j < 0) continue;
+                  multiply_At_v(Bk[k], residuals[k], tmp);
+                  addVectors(tmp, Bt_e[j], Bt_e[j]);
+               } // end for (k)
+            } // end if
+
+            if (nVaryingC > 0)
+            {
+               makeZeroVector(Ct_e);
+
+               Vector<double> tmp(_paramDimensionC);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  multiply_At_v(Ck[k], residuals[k], tmp);
+                  for (int l = 0; l < nVaryingC; ++l) Ct_e[l] += tmp[_nNonvaryingC + l];
+               }
+            } // end if
+
+            int pos = 0;
+            for (int i = 0; i < nVaryingA; ++i)
+               for (int l = 0; l < _paramDimensionA; ++l, ++pos)
+                  Jt_e[pos] = At_e[i][l];
+            for (int j = 0; j < nVaryingB; ++j)
+               for (int l = 0; l < _paramDimensionB; ++l, ++pos)
+                  Jt_e[pos] = Bt_e[j][l];
+            for (int l = 0; l < nVaryingC; ++l, ++pos)
+               Jt_e[pos] = Ct_e[l];
+
+//                cout << "Jt_e = ";
+//                for (int k = 0; k < Jt_e.size(); ++k) cout << Jt_e[k] << " ";
+//                cout << endl;
+
+            if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e)))
+            {
+               status = LEVENBERG_OPTIMIZER_CONVERGED;
+               goto end;
+            }
+
+            // The lhs J^t*J consists of several parts:
+            //         [ U     W   X ]
+            // J^t*J = [ W^t   V   Y ]
+            //         [ X^t  Y^t  Z ],
+            // where U, V and W are block-sparse matrices (due to the sparsity of A and B).
+            // X, Y and Z contain only a few columns (the number of global parameters).
+
+            if (nVaryingA > 0)
+            {
+               // Compute Ui
+               Matrix<double> U(_paramDimensionA, _paramDimensionA);
+
+               for (int i = 0; i < nVaryingA; ++i) makeZeroMatrix(Ui[i]);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const i = _correspondingParamA[k] - _nNonvaryingA;
+                  if (i < 0) continue;
+                  multiply_At_A(Ak[k], U);
+                  addMatricesIP(U, Ui[i]);
+               } // end for (k)
+            } // end if
+
+            if (nVaryingB > 0)
+            {
+               // Compute Vj
+               Matrix<double> V(_paramDimensionB, _paramDimensionB);
+
+               for (int j = 0; j < nVaryingB; ++j) makeZeroMatrix(Vj[j]);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const j = _correspondingParamB[k] - _nNonvaryingB;
+                  if (j < 0) continue;
+                  multiply_At_A(Bk[k], V);
+                  addMatricesIP(V, Vj[j]);
+               } // end for (k)
+            } // end if
+
+            if (nVaryingC > 0)
+            {
+               Matrix<double> ZZ(_paramDimensionC, _paramDimensionC);
+               Matrix<double> Zsum(_paramDimensionC, _paramDimensionC);
+
+               makeZeroMatrix(Zsum);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  multiply_At_A(Ck[k], ZZ);
+                  addMatricesIP(ZZ, Zsum);
+               } // end for (k)
+
+               // Ignore the non-varying parameters
+               for (int i = 0; i < nVaryingC; ++i)
+                  for (int j = 0; j < nVaryingC; ++j)
+                     Z[i][j] = Zsum[i+_nNonvaryingC][j+_nNonvaryingC];
+            } // end if
+
+            if (nVaryingA > 0 && nVaryingB > 0)
+            {
+               for (int n = 0; n < Wn.count(); ++n) makeZeroMatrix(Wn[n]);
+
+               Matrix<double> W(_paramDimensionA, _paramDimensionB);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const n = _jointIndexW[k];
+                  if (n >= 0)
+                  {
+                     int const i0 = _jointNonzerosW[n].first;
+                     int const j0 = _jointNonzerosW[n].second;
+
+                     multiply_At_B(Ak[k], Bk[k], W);
+                     addMatricesIP(W, Wn[n]);
+                  } // end if
+               } // end for (k)
+            } // end if
+
+            if (nVaryingA > 0 && nVaryingC > 0)
+            {
+               Matrix<double> XX(_paramDimensionA, _paramDimensionC);
+
+               makeZeroMatrix(X);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const i = _correspondingParamA[k] - _nNonvaryingA;
+                  // Ignore the non-varying parameters
+                  if (i < 0) continue;
+
+                  multiply_At_B(Ak[k], Ck[k], XX);
+
+                  for (int r = 0; r < _paramDimensionA; ++r)
+                     for (int c = 0; c < nVaryingC; ++c)
+                        X[r+i*_paramDimensionA][c] += XX[r][c+_nNonvaryingC];
+               } // end for (k)
+            } // end if
+
+            if (nVaryingB > 0 && nVaryingC > 0)
+            {
+               Matrix<double> YY(_paramDimensionB, _paramDimensionC);
+
+               makeZeroMatrix(Y);
+
+               for (int k = 0; k < _nMeasurements; ++k)
+               {
+                  int const j = _correspondingParamB[k] - _nNonvaryingB;
+                  // Ignore the non-varying parameters
+                  if (j < 0) continue;
+
+                  multiply_At_B(Bk[k], Ck[k], YY);
+
+                  for (int r = 0; r < _paramDimensionB; ++r)
+                     for (int c = 0; c < nVaryingC; ++c)
+                        Y[r+j*_paramDimensionB][c] += YY[r][c+_nNonvaryingC];
+               } // end for (k)
+            } // end if
+
+            if (currentIteration == 0)
+            {
+               // Initialize lambda as tau*max(JtJ[i][i])
+               double maxEl = -1e30;
+               if (nVaryingA > 0)
+               {
+                  for (int i = 0; i < nVaryingA; ++i)
+                     for (int l = 0; l < _paramDimensionA; ++l)
+                        maxEl = std::max(maxEl, Ui[i][l][l]);
+               }
+               if (nVaryingB > 0)
+               {
+                  for (int j = 0; j < nVaryingB; ++j)
+                     for (int l = 0; l < _paramDimensionB; ++l)
+                        maxEl = std::max(maxEl, Vj[j][l][l]);
+               }
+               if (nVaryingC > 0)
+               {
+                  for (int l = 0; l < nVaryingC; ++l)
+                     maxEl = std::max(maxEl, Z[l][l]);
+               }
+
+               lambda = tau * maxEl;
+               if (optimizerVerbosenessLevel >= 2)
+                  cout << "SparseLevenbergOptimizer: initial lambda = " << lambda << endl;
+            } // end if (currentIteration == 0)
+         } // end if (computeDerivatives)
+
+         for (int i = 0; i < nVaryingA; ++i)
+         {
+            for (int l = 0; l < _paramDimensionA; ++l) diagUi[i][l] = Ui[i][l][l];
+         } // end for (i)
+
+         for (int j = 0; j < nVaryingB; ++j)
+         {
+            for (int l = 0; l < _paramDimensionB; ++l) diagVj[j][l] = Vj[j][l][l];
+         } // end for (j)
+
+         for (int l = 0; l < nVaryingC; ++l) diagZ[l] = Z[l][l];
+
+         // Augment the diagonals with lambda (either by the standard additive update or by multiplication).
+#if !defined(USE_MULTIPLICATIVE_UPDATE)
+         for (int i = 0; i < nVaryingA; ++i)
+            for (unsigned l = 0; l < _paramDimensionA; ++l)
+               Ui[i][l][l] += lambda;
+
+         for (int j = 0; j < nVaryingB; ++j)
+            for (unsigned l = 0; l < _paramDimensionB; ++l)
+               Vj[j][l][l] += lambda;
+
+         for (unsigned l = 0; l < nVaryingC; ++l)
+            Z[l][l] += lambda;
+#else
+         for (int i = 0; i < nVaryingA; ++i)
+            for (unsigned l = 0; l < _paramDimensionA; ++l)
+               Ui[i][l][l] = std::max(Ui[i][l][l] * (1.0 + lambda), 1e-15);
+
+         for (int j = 0; j < nVaryingB; ++j)
+            for (unsigned l = 0; l < _paramDimensionB; ++l)
+               Vj[j][l][l] = std::max(Vj[j][l][l] * (1.0 + lambda), 1e-15);
+
+         for (unsigned l = 0; l < nVaryingC; ++l)
+            Z[l][l] = std::max(Z[l][l] * (1.0 + lambda), 1e-15);
+#endif
+
+         this->fillSparseJtJ(Ui, Vj, Wn, Z, X, Y);
+
+         bool success = true;
+         double rho = 0.0;
+         {
+            int const nCols = _JtJ_Parent.size();
+            int const nnz   = _JtJ.getNonzeroCount();
+            int const lnz   = _JtJ_Lp.back();
+
+            vector<int> Li(lnz);
+            vector<double> Lx(lnz);
+            vector<double> D(nCols), Y(nCols);
+            vector<int> workPattern(nCols), workFlag(nCols);
+
+            int * colStarts = (int *)_JtJ.getColumnStarts();
+            int * rowIdxs   = (int *)_JtJ.getRowIndices();
+            double * values = _JtJ.getValues();
+
+            int const d = ldl_numeric(nCols, colStarts, rowIdxs, values,
+                                      &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0],
+                                      &Li[0], &Lx[0], &D[0],
+                                      &Y[0], &workPattern[0], &workFlag[0],
+                                      NULL, NULL);
+
+            if (d == nCols)
+            {
+               ldl_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]);
+               ldl_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]);
+               ldl_dsolve(nCols, &deltaPerm[0], &D[0]);
+               ldl_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]);
+               ldl_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]);
+            }
+            else
+            {
+               if (optimizerVerbosenessLevel >= 2)
+                  cout << "SparseLevenbergOptimizer: LDL decomposition failed. Increasing lambda." << endl;
+               success = false;
+            }
+         }
+
+         if (success)
+         {
+            double const deltaSqrLength = sqrNorm_L2(delta);
+
+            if (optimizerVerbosenessLevel >= 2)
+               cout << "SparseLevenbergOptimizer: ||delta||^2 = " << deltaSqrLength << endl;
+
+            double const paramLength = this->getParameterLength();
+            if (this->applyUpdateStoppingCriteria(paramLength, sqrt(deltaSqrLength)))
+            {
+               status = LEVENBERG_OPTIMIZER_SMALL_UPDATE;
+               goto end;
+            }
+
+            // Copy the updates from delta to the respective arrays
+            int pos = 0;
+
+            for (int i = 0; i < _nNonvaryingA; ++i) makeZeroVector(deltaAi[i]);
+            for (int i = _nNonvaryingA; i < _nParametersA; ++i)
+               for (int l = 0; l < _paramDimensionA; ++l, ++pos)
+                  deltaAi[i][l] = delta[pos];
+
+            for (int j = 0; j < _nNonvaryingB; ++j) makeZeroVector(deltaBj[j]);
+            for (int j = _nNonvaryingB; j < _nParametersB; ++j)
+               for (int l = 0; l < _paramDimensionB; ++l, ++pos)
+                  deltaBj[j][l] = delta[pos];
+
+            makeZeroVector(deltaC);
+            for (int l = _nNonvaryingC; l < _paramDimensionC; ++l, ++pos)
+               deltaC[l] = delta[pos];
+
+            saveAllParameters();
+            if (nVaryingA > 0) updateParametersA(deltaAi);
+            if (nVaryingB > 0) updateParametersB(deltaBj);
+            if (nVaryingC > 0) updateParametersC(deltaC);
+
+            this->evalResidual(residuals2);
+            for (int k = 0; k < _nMeasurements; ++k)
+               scaleVectorIP(weights[k], residuals2[k]);
+
+            double const newErr = squaredResidual(residuals2);
+            rho = err - newErr;
+            if (optimizerVerbosenessLevel >= 2)
+               cout << "SparseLevenbergOptimizer: |new residual|^2 = " << newErr << endl;
+
+#if !defined(USE_MULTIPLICATIVE_UPDATE)
+            double const denom1 = lambda * deltaSqrLength;
+#else
+            double denom1 = 0.0f;
+            for (int i = _nNonvaryingA; i < _nParametersA; ++i)
+               for (int l = 0; l < _paramDimensionA; ++l)
+                  denom1 += deltaAi[i][l] * deltaAi[i][l] * diagUi[i-_nNonvaryingA][l];
+
+            for (int j = _nNonvaryingB; j < _nParametersB; ++j)
+               for (int l = 0; l < _paramDimensionB; ++l)
+                  denom1 += deltaBj[j][l] * deltaBj[j][l] * diagVj[j-_nNonvaryingB][l];
+
+            for (int l = _nNonvaryingC; l < _paramDimensionC; ++l)
+               denom1 += deltaC[l] * deltaC[l] * diagZ[l-_nNonvaryingC];
+
+            denom1 *= lambda;
+#endif
+            double const denom2 = innerProduct(delta, Jt_e);
+            rho = rho / (denom1 + denom2);
+            if (optimizerVerbosenessLevel >= 2)
+               cout << "SparseLevenbergOptimizer: rho = " << rho
+                    << " denom1 = " << denom1 << " denom2 = " << denom2 << endl;
+         } // end if (success)
+
+         if (success && rho > 0)
+         {
+            if (optimizerVerbosenessLevel >= 2)
+               cout << "SparseLevenbergOptimizer: Improved solution - decreasing lambda." << endl;
+            // Improvement in the new solution
+            decreaseLambda(rho);
+            computeDerivatives = true;
+         }
+         else
+         {
+            if (optimizerVerbosenessLevel >= 2)
+               cout << "SparseLevenbergOptimizer: Inferior solution - increasing lambda." << endl;
+            restoreAllParameters();
+            increaseLambda();
+            computeDerivatives = false;
+
+            // Restore diagonal elements in Ui, Vj and Z.
+            for (int i = 0; i < nVaryingA; ++i)
+            {
+               for (int l = 0; l < _paramDimensionA; ++l) Ui[i][l][l] = diagUi[i][l];
+            } // end for (i)
+
+            for (int j = 0; j < nVaryingB; ++j)
+            {
+               for (int l = 0; l < _paramDimensionB; ++l) Vj[j][l][l] = diagVj[j][l];
+            } // end for (j)
+
+            for (int l = 0; l < nVaryingC; ++l) Z[l][l] = diagZ[l];
+         } // end if
+      } // end for
+
+     end:;
+      if (optimizerVerbosenessLevel >= 2)
+         cout << "Leaving SparseLevenbergOptimizer::minimize()." << endl;
+   } // end SparseLevenbergOptimizer::minimize()
+
+#endif // defined(V3DLIB_ENABLE_SUITESPARSE)
+
+} // end namespace V3D
diff --git a/extern/libmv/third_party/ssba/Math/v3d_optimization.h b/extern/libmv/third_party/ssba/Math/v3d_optimization.h
new file mode 100644
index 00000000000..27d2e12287f
--- /dev/null
+++ b/extern/libmv/third_party/ssba/Math/v3d_optimization.h
@@ -0,0 +1,273 @@
+// -*- C++ -*-
+/*
+Copyright (c) 2008 University of North Carolina at Chapel Hill
+
+This file is part of SSBA (Simple Sparse Bundle Adjustment).
+
+SSBA is free software: you can redistribute it and/or modify it under the
+terms of the GNU Lesser General Public License as published by the Free
+Software Foundation, either version 3 of the License, or (at your option) any
+later version.
+
+SSBA 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 Lesser General Public License for more
+details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with SSBA. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef V3D_OPTIMIZATION_H
+#define V3D_OPTIMIZATION_H
+
+#include "Math/v3d_linear.h"
+#include "Math/v3d_mathutilities.h"
+
+#include <vector>
+#include <iostream>
+
+namespace V3D
+{
+
+   enum
+   {
+      LEVENBERG_OPTIMIZER_TIMEOUT = 0,
+      LEVENBERG_OPTIMIZER_SMALL_UPDATE = 1,
+      LEVENBERG_OPTIMIZER_CONVERGED = 2
+   };
+
+   extern int optimizerVerbosenessLevel;
+
+   struct LevenbergOptimizerCommon
+   {
+         LevenbergOptimizerCommon()
+            : status(LEVENBERG_OPTIMIZER_TIMEOUT), currentIteration(0), maxIterations(50),
+              tau(1e-3), lambda(1e-3),
+              gradientThreshold(1e-10), updateThreshold(1e-10),
+              _nu(2.0)
+         { }
+         virtual ~LevenbergOptimizerCommon() {}
+
+         // See Madsen et al., "Methods for non-linear least squares problems."
+         virtual void increaseLambda()
+         {
+            lambda *= _nu; _nu *= 2.0;
+         }
+
+         virtual void decreaseLambda(double const rho)
+         {
+            double const r = 2*rho - 1.0;
+            lambda *= std::max(1.0/3.0, 1 - r*r*r);
+            if (lambda < 1e-10) lambda = 1e-10;
+            _nu = 2;
+         }
+
+         bool applyGradientStoppingCriteria(double maxGradient) const
+         {
+            return maxGradient < gradientThreshold;
+         }
+
+         bool applyUpdateStoppingCriteria(double paramLength, double updateLength) const
+         {
+            return updateLength < updateThreshold * (paramLength + updateThreshold);
+         }
+
+         int    status;
+         int    currentIteration, maxIterations;
+         double tau, lambda;
+         double gradientThreshold, updateThreshold;
+
+      protected:
+         double _nu;
+   }; // end struct LevenbergOptimizerCommon
+
+# if defined(V3DLIB_ENABLE_SUITESPARSE)
+
+   struct SparseLevenbergOptimizer : public LevenbergOptimizerCommon
+   {
+         SparseLevenbergOptimizer(int measurementDimension,
+                                  int nParametersA, int paramDimensionA,
+                                  int nParametersB, int paramDimensionB,
+                                  int paramDimensionC,
+                                  std::vector<int> const& correspondingParamA,
+                                  std::vector<int> const& correspondingParamB)
+            : LevenbergOptimizerCommon(),
+              _nMeasurements(correspondingParamA.size()),
+              _measurementDimension(measurementDimension),
+              _nParametersA(nParametersA), _paramDimensionA(paramDimensionA),
+              _nParametersB(nParametersB), _paramDimensionB(paramDimensionB),
+              _paramDimensionC(paramDimensionC),
+              _nNonvaryingA(0), _nNonvaryingB(0), _nNonvaryingC(0),
+              _correspondingParamA(correspondingParamA),
+              _correspondingParamB(correspondingParamB)
+         {
+            assert(correspondingParamA.size() == correspondingParamB.size());
+         }
+
+         ~SparseLevenbergOptimizer() { }
+
+         void setNonvaryingCounts(int nNonvaryingA, int nNonvaryingB, int nNonvaryingC)
+         {
+            _nNonvaryingA = nNonvaryingA;
+            _nNonvaryingB = nNonvaryingB;
+            _nNonvaryingC = nNonvaryingC;
+         }
+
+         void getNonvaryingCounts(int& nNonvaryingA, int& nNonvaryingB, int& nNonvaryingC) const
+         {
+            nNonvaryingA = _nNonvaryingA;
+            nNonvaryingB = _nNonvaryingB;
+            nNonvaryingC = _nNonvaryingC;
+         }
+
+         void minimize();
+
+         virtual void evalResidual(VectorArray<double>& residuals) = 0;
+
+         virtual void fillWeights(VectorArray<double> const& residuals, Vector<double>& w)
+         {
+            (void)residuals;
+            std::fill(w.begin(), w.end(), 1.0);
+         }
+
+         void fillAllJacobians(Vector<double> const& w,
+                               MatrixArray<double>& Ak,
+                               MatrixArray<double>& Bk,
+                               MatrixArray<double>& Ck)
+         {
+            int const nVaryingA = _nParametersA - _nNonvaryingA;
+            int const nVaryingB = _nParametersB - _nNonvaryingB;
+            int const nVaryingC = _paramDimensionC - _nNonvaryingC;
+
+            for (unsigned k = 0; k < _nMeasurements; ++k)
+            {
+               int const i = _correspondingParamA[k];
+               int const j = _correspondingParamB[k];
+
+               if (i < _nNonvaryingA && j < _nNonvaryingB) continue;
+
+               fillJacobians(Ak[k], Bk[k], Ck[k], i, j, k);
+            } // end for (k)
+
+            if (nVaryingA > 0)
+            {
+               for (unsigned k = 0; k < _nMeasurements; ++k)
+                  scaleMatrixIP(w[k], Ak[k]);
+            }
+            if (nVaryingB > 0)
+            {
+               for (unsigned k = 0; k < _nMeasurements; ++k)
+                  scaleMatrixIP(w[k], Bk[k]);
+            }
+            if (nVaryingC > 0)
+            {
+               for (unsigned k = 0; k < _nMeasurements; ++k)
+                  scaleMatrixIP(w[k], Ck[k]);
+            }
+         } // end fillAllJacobians()
+
+         virtual void setupJacobianGathering() { }
+
+         virtual void fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
+                                    int i, int j, int k) = 0;
+
+         virtual double getParameterLength() const = 0;
+
+         virtual void updateParametersA(VectorArray<double> const& deltaAi) = 0;
+         virtual void updateParametersB(VectorArray<double> const& deltaBj) = 0;
+         virtual void updateParametersC(Vector<double> const& deltaC) = 0;
+         virtual void saveAllParameters() = 0;
+         virtual void restoreAllParameters() = 0;
+
+         int currentIteration, maxIterations;
+
+      protected:
+         void serializeNonZerosJtJ(std::vector<std::pair<int, int> >& dst) const;
+         void setupSparseJtJ();
+         void fillSparseJtJ(MatrixArray<double> const& Ui, MatrixArray<double> const& Vj, MatrixArray<double> const& Wk,
+                            Matrix<double> const& Z, Matrix<double> const& X, Matrix<double> const& Y);
+
+         int const _nMeasurements, _measurementDimension;
+         int const _nParametersA, _paramDimensionA;
+         int const _nParametersB, _paramDimensionB;
+         int const _paramDimensionC;
+
+         int _nNonvaryingA, _nNonvaryingB, _nNonvaryingC;
+
+         std::vector<int> const& _correspondingParamA;
+         std::vector<int> const& _correspondingParamB;
+
+         std::vector<pair<int, int> > _jointNonzerosW;
+         std::vector<int>             _jointIndexW;
+
+         std::vector<int> _JtJ_Lp, _JtJ_Parent, _JtJ_Lnz;
+         std::vector<int> _perm_JtJ, _invPerm_JtJ;
+
+         CCS_Matrix<double> _JtJ;
+   }; // end struct SparseLevenbergOptimizer
+
+   struct StdSparseLevenbergOptimizer : public SparseLevenbergOptimizer
+   {
+         StdSparseLevenbergOptimizer(int measurementDimension,
+                                     int nParametersA, int paramDimensionA,
+                                     int nParametersB, int paramDimensionB,
+                                     int paramDimensionC,
+                                     std::vector<int> const& correspondingParamA,
+                                     std::vector<int> const& correspondingParamB)
+            : SparseLevenbergOptimizer(measurementDimension, nParametersA, paramDimensionA,
+                                       nParametersB, paramDimensionB, paramDimensionC,
+                                       correspondingParamA, correspondingParamB),
+              curParametersA(nParametersA, paramDimensionA), savedParametersA(nParametersA, paramDimensionA),
+              curParametersB(nParametersB, paramDimensionB), savedParametersB(nParametersB, paramDimensionB),
+              curParametersC(paramDimensionC), savedParametersC(paramDimensionC)
+         { }
+
+         virtual double getParameterLength() const
+         {
+            double res = 0.0;
+            for (int i = 0; i < _nParametersA; ++i) res += sqrNorm_L2(curParametersA[i]);
+            for (int j = 0; j < _nParametersB; ++j) res += sqrNorm_L2(curParametersB[j]);
+            res += sqrNorm_L2(curParametersC);
+            return sqrt(res);
+         }
+
+         virtual void updateParametersA(VectorArray<double> const& deltaAi)
+         {
+            for (int i = 0; i < _nParametersA; ++i) addVectors(deltaAi[i], curParametersA[i], curParametersA[i]);
+         }
+
+         virtual void updateParametersB(VectorArray<double> const& deltaBj)
+         {
+            for (int j = 0; j < _nParametersB; ++j) addVectors(deltaBj[j], curParametersB[j], curParametersB[j]);
+         }
+
+         virtual void updateParametersC(Vector<double> const& deltaC)
+         {
+            addVectors(deltaC, curParametersC, curParametersC);
+         }
+
+         virtual void saveAllParameters()
+         {
+            for (int i = 0; i < _nParametersA; ++i) savedParametersA[i] = curParametersA[i];
+            for (int j = 0; j < _nParametersB; ++j) savedParametersB[j] = curParametersB[j];
+            savedParametersC = curParametersC;
+         }
+
+         virtual void restoreAllParameters()
+         {
+            for (int i = 0; i < _nParametersA; ++i) curParametersA[i] = savedParametersA[i];
+            for (int j = 0; j < _nParametersB; ++j) curParametersB[j] = savedParametersB[j];
+            curParametersC = savedParametersC;
+         }
+
+         VectorArray<double> curParametersA, savedParametersA;
+         VectorArray<double> curParametersB, savedParametersB;
+         Vector<double>      curParametersC, savedParametersC;
+   }; // end struct StdSparseLevenbergOptimizer
+
+# endif
+
+} // end namespace V3D
+
+#endif