v3d_optimization.cpp

A version of a sparse bundle adjustment implementation with adjustable intrinsics and distortion par

/*
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