v3d_linear.h

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

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

//----------------------------------------------------------------------