v3d_linear.h

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

   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