示例#1
0
   void
   MetricBundleOptimizerBase::poseDerivatives(int i, int j, Vector3d& XX,
                                              Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const
   {
      XX = _cams[i].transformPointIntoCameraSpace(_Xs[j]);

      // See Frank Dellaerts bundle adjustment tutorial.
      // d(dR * R0 * X + t)/d omega = -[R0 * X]_x
      Matrix3x3d J;
      makeCrossProductMatrix(XX - _cams[i].getTranslation(), J);
      scaleMatrixIP(-1.0, J);

      // Now the transformation from world coords into camera space is xx = Rx + T
      // Hence the derivative of x wrt. T is just the identity matrix.
      makeIdentityMatrix(d_dRT);
      copyMatrixSlice(J, 0, 0, 3, 3, d_dRT, 0, 3);

      // The derivative of Rx+T wrt x is just R.
      copyMatrix(_cams[i].getRotation(), d_dX);
   } // end MetricBundleOptimizerBase::poseDerivatives()
   void
   computeConsistentRotations_LSQ(int const nIterations, int const nViews,
                                  std::vector<Matrix3x3d> const& relativeRotations,
                                  std::vector<std::pair<int, int> > const& viewPairs,
                                  std::vector<Matrix3x3d>& rotations)
   {
      double const gamma = 1.0;

      int const nRelPoses = relativeRotations.size();

      rotations.resize(nViews);

      Matrix3x3d zero3x3d;
      makeZeroMatrix(zero3x3d);

      Matrix4x4d zeroQuat;
      makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;

      vector<double> denomQ(nViews, 1.0); // from the psd constraint
      for (int k = 0; k < nRelPoses; ++k)
      {
         int const i = viewPairs[k].first;
         int const j = viewPairs[k].second;
         denomQ[i] += 1;
         denomQ[j] += 1;
      }
      for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];

      vector<Matrix4x4d> Q(nViews, zeroQuat);
      vector<Matrix4x4d> Q1(nViews, zeroQuat);
      vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
      vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);

      vector<Matrix3x3d> A(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);

      for (int iter = 0; iter < nIterations; ++iter)
      {
         // Convex hull of rotation matrices
         for (int i = 0; i < nViews; ++i)
         {
            Matrix4x4d q = Q[i] + ZQ1[i];
            projectConvHull_SO3(q);
            Q1[i] = q;
            addMatricesIP(Q[i] - q, ZQ1[i]);
         } // end for (i)

         // Squared Frobenius term 0.5*sum_k |A_k|_F^2
         for (int k = 0; k < nRelPoses; ++k)
         {
            Matrix3x3d a = A[k] + ZA1[k];
            scaleMatrixIP(1.0 / (1.0 + gamma), a);
            A1[k] = a;
            addMatricesIP(A[k] - a, ZA1[k]);
         } // end for (k)

         // Enforce linear consistency
         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            Matrix4x4d qi = Q[i] + ZQ2i[k];
            Matrix4x4d qj = Q[j] + ZQ2j[k];
            Matrix3x3d a = A[k] + ZA2[k];

            proxConsistency(relativeRotations[k], qi, qj, a);

            Q2i[k] = qi;
            Q2j[k] = qj;
            A2[k] = a;
            addMatricesIP(Q[i] - qi, ZQ2i[k]);
            addMatricesIP(Q[j] - qj, ZQ2j[k]);
            addMatricesIP(A[k] - a, ZA2[k]);
         } // end for (k)

         // Averaging of the solutions
         for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];

         for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];

         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);
            addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);
            addMatricesIP(A2[k] - ZA2[k], A[k]);
         } // end for (k)

         for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);
         for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);

         if ((iter % 500) == 0)
         {
            double E = 0;
            for (int k = 0; k < nRelPoses; ++k) E += sqrMatrixNormFrobenius(A[k]);
            cout << "iter: " << iter << " E = " << E << endl;
         }
      } // end for (iter)

      rotations.resize(nViews);
      for (int i = 0; i < nViews; ++i)
         rotations[i] = getRotationFromQuat(Q[i]);
   } // end computeConsistentRotations_LSQ()
   void
   computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews,
                                   std::vector<Matrix3x3d> const& relativeRotations,
                                   std::vector<std::pair<int, int> > const& viewPairs,
                                   std::vector<Matrix3x3d>& rotations, std::vector<double>& zs)
   {
      double const gamma = 1.0;

      int const nRelPoses = relativeRotations.size();

      rotations.resize(nViews);

      Matrix3x3d zero3x3d;
      makeZeroMatrix(zero3x3d);

      Matrix4x4d zeroQuat;
      makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;

      double const denomT = 1.0 / (1.0 + nRelPoses);

      vector<double> denomQ(nViews, 1.0); // from the psd constraint
      for (int k = 0; k < nRelPoses; ++k)
      {
         int const i = viewPairs[k].first;
         int const j = viewPairs[k].second;
         denomQ[i] += 1;
         denomQ[j] += 1;
      }
      for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];

      double T = 0.0;
      vector<double> T1(nRelPoses);
      vector<double> ZT1(nRelPoses, 0.0);
      double T2;
      double ZT2 = 0;

      vector<Matrix4x4d> Q(nViews, zeroQuat);
      vector<Matrix4x4d> Q1(nViews, zeroQuat);
      vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
      vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);

      vector<Matrix3x3d> A(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);

      for (int iter = 0; iter < nIterations; ++iter)
      {
         // Convex hull of rotation matrices
         for (int i = 0; i < nViews; ++i)
         {
            Matrix4x4d q = Q[i] + ZQ1[i];
            if (i > 0)
               projectConvHull_SO3(q);
            else
            {
               makeZeroMatrix(q);
               q[0][0] = 1;
            }
            Q1[i] = q;
            addMatricesIP(Q[i] - q, ZQ1[i]);
         } // end for (i)

         // Shrinkage of T (we want to minimize T)
         T2 = std::max(0.0, T + ZT2 - gamma);
         ZT2 += T - T2;

         // Cone constraint
         for (int k = 0; k < nRelPoses; ++k)
         {
            double t = T + ZT1[k];
            Matrix3x3d a = A[k] + ZA1[k];

            proxDataResidual_Frobenius(sigma, t, a);

            T1[k] = t;
            ZT1[k] += T - t;
            A1[k] = a;
            addMatricesIP(A[k] - a, ZA1[k]);
         } // end for (k)

         // Enforce linear consistency
         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            Matrix4x4d qi = Q[i] + ZQ2i[k];
            Matrix4x4d qj = Q[j] + ZQ2j[k];
            Matrix3x3d a = A[k] + ZA2[k];

            proxConsistency(relativeRotations[k], qi, qj, a);

            Q2i[k] = qi;
            Q2j[k] = qj;
            A2[k] = a;
            addMatricesIP(Q[i] - qi, ZQ2i[k]);
            addMatricesIP(Q[j] - qj, ZQ2j[k]);
            addMatricesIP(A[k] - a, ZA2[k]);
         } // end for (k)

         // Averaging of the solutions
         for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];

         T = T2 - ZT2;
         for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k];
         T *= denomT;
         T = std::max(0.0, T);

         for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];

         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);
            addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);
            addMatricesIP(A2[k] - ZA2[k], A[k]);
         } // end for (k)

         for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);
         for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);

         if ((iter % 500) == 0)
         {
            cout << "iter: " << iter << " t = " << T << endl;
            cout << "T1 = "; displayVector(T1);
            cout << "ZT1 = "; displayVector(ZT1);
            cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl;

            Matrix<double> ZZ(4, 4);
            for (int i = 0; i < nViews; ++i)
            {
               copyMatrix(Q[i], ZZ);
               SVD<double> svd(ZZ);
               cout << "Q = "; displayMatrix(ZZ);
               cout << "SV = "; displayVector(svd.getSingularValues());
               //Matrix3x3d R = getRotationFromQuat(Q[i]);
               //cout << "R = "; displayMatrix(R);
            } // end for (i)
         }
      } // end for (iter)

      rotations.resize(nViews);
      for (int i = 0; i < nViews; ++i)
         rotations[i] = getRotationFromQuat(Q[i]);

      zs = ZT1;
   } // end computeConsistentRotations_Linf()
   void
   computeConsistentRotations_L1(double const sigma, int const nIterations, int const nViews,
                                 std::vector<Matrix3x3d> const& relativeRotations,
                                 std::vector<std::pair<int, int> > const& viewPairs,
                                 std::vector<Matrix3x3d>& rotations)
   {
      double const gamma = 1.0;

      int const nRelPoses = relativeRotations.size();

      rotations.resize(nViews);

      Matrix3x3d zero3x3d;
      makeZeroMatrix(zero3x3d);

      Matrix4x4d zeroQuat;
      makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;

      vector<double> denomQ(nViews, 1.0); // from the psd constraint
      for (int k = 0; k < nRelPoses; ++k)
      {
         int const i = viewPairs[k].first;
         int const j = viewPairs[k].second;
         denomQ[i] += 1;
         denomQ[j] += 1;
      }
      for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];

      vector<double> T(nRelPoses, 0.0);
      vector<double> T1(nRelPoses);
      vector<double> ZT1(nRelPoses, 0.0);
      vector<double> T2(nRelPoses);
      vector<double> ZT2(nRelPoses, 0.0);

      vector<Matrix4x4d> Q(nViews, zeroQuat);
      vector<Matrix4x4d> Q1(nViews, zeroQuat);
      vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
      vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
      vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);

      vector<Matrix3x3d> A(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
      vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);

      for (int iter = 0; iter < nIterations; ++iter)
      {
         // Convex hull of rotation matrices
         for (int i = 0; i < nViews; ++i)
         {
            Matrix4x4d q = Q[i] + ZQ1[i];
            projectConvHull_SO3(q);
            Q1[i] = q;
            addMatricesIP(Q[i] - q, ZQ1[i]);
         } // end for (i)

         // Shrinkage of T (we want to minimize T)
         for (int k = 0; k < nRelPoses; ++k)
         {
            T2[k] = std::max(0.0, T[k] + ZT2[k] - gamma);
            ZT2[k] += T[k] - T2[k];
         } // end for (k)

         // Cone constraint
         for (int k = 0; k < nRelPoses; ++k)
         {
            double t = T1[k] + ZT1[k];
            Matrix3x3d a = A[k] + ZA1[k];

            proxDataResidual_Frobenius(sigma, t, a);

            T1[k] = t;
            ZT1[k] += T[k] - t;
            A1[k] = a;
            addMatricesIP(A[k] - a, ZA1[k]);
         } // end for (k)

         // Enforce linear consistency
         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            Matrix4x4d qi = Q[i] + ZQ2i[k];
            Matrix4x4d qj = Q[j] + ZQ2j[k];
            Matrix3x3d a = A[k] + ZA2[k];

            proxConsistency(relativeRotations[k], qi, qj, a);

            Q2i[k] = qi;
            Q2j[k] = qj;
            A2[k] = a;
            addMatricesIP(Q[i] - qi, ZQ2i[k]);
            addMatricesIP(Q[j] - qj, ZQ2j[k]);
            addMatricesIP(A[k] - a, ZA2[k]);
         } // end for (k)

         // Averaging of the solutions
         for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];

         for (int k = 0; k < nRelPoses; ++k)
            T[k] = std::max(0.0, 0.5 * (T1[k] - ZT1[k] + T2[k] - ZT2[k]));

         for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];

         for (int k = 0; k < nRelPoses; ++k)
         {
            int const i = viewPairs[k].first;
            int const j = viewPairs[k].second;

            addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);
            addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);
            addMatricesIP(A2[k] - ZA2[k], A[k]);
         } // end for (k)

         for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);
         for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);

         if ((iter % 500) == 0)
         {
            cout << "iter: " << iter << endl;
            cout << " T = "; displayVector(T);
         }
      } // end for (iter)

      rotations.resize(nViews);
      for (int i = 0; i < nViews; ++i)
         rotations[i] = getRotationFromQuat(Q[i]);
   } // end computeConsistentRotations_L1()
   void
   computeConsistentRotations(int const nViews,
                              std::vector<Matrix3x3d> const& relativeRotations,
                              std::vector<std::pair<int, int> > const& viewPairs,
                              std::vector<Matrix3x3d>& rotations, int method)
   {
#if !defined(V3DLIB_ENABLE_ARPACK)
      if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
         method = V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA;
#endif

      int const nRelPoses = relativeRotations.size();

      rotations.resize(nViews);

      switch (method)
      {
         case V3D_CONSISTENT_ROTATION_METHOD_SVD:
         {
            Matrix<double> A(3*nRelPoses, 3*nViews, 0.0);
            Matrix3x3d I;
            makeIdentityMatrix(I);
            scaleMatrixIP(-1.0, I);

            for (int i = 0; i < nRelPoses; ++i)
            {
               int const view1 = viewPairs[i].first;
               int const view2 = viewPairs[i].second;

               Matrix3x3d const& Rrel = relativeRotations[i];

               copyMatrixSlice(Rrel, 0, 0, 3, 3, A, 3*i, 3*view1);
               copyMatrixSlice(I,    0, 0, 3, 3, A, 3*i, 3*view2);
            } // end for (i)

            SVD<double> svd(A);
            int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD

            Matrix<double> const& V = svd.getV();

            for (int i = 0; i < nViews; ++i)
            {
               copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
               enforceRotationMatrix(rotations[i]);
            }
            break;
         }
         case V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA:
         case V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA:
         case V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG:
         {
            vector<pair<int, int> > nzA;
            vector<double> valsA;
            nzA.reserve(12*nRelPoses);
            valsA.reserve(12*nRelPoses);

            for (int i = 0; i < nRelPoses; ++i)
            {
               int const view1 = viewPairs[i].first;
               int const view2 = viewPairs[i].second;

               Matrix3x3d const& Rrel = relativeRotations[i];

               nzA.push_back(make_pair(3*i+0, 3*view1+0)); valsA.push_back(Rrel[0][0]);
               nzA.push_back(make_pair(3*i+0, 3*view1+1)); valsA.push_back(Rrel[0][1]);
               nzA.push_back(make_pair(3*i+0, 3*view1+2)); valsA.push_back(Rrel[0][2]);
               nzA.push_back(make_pair(3*i+1, 3*view1+0)); valsA.push_back(Rrel[1][0]);
               nzA.push_back(make_pair(3*i+1, 3*view1+1)); valsA.push_back(Rrel[1][1]);
               nzA.push_back(make_pair(3*i+1, 3*view1+2)); valsA.push_back(Rrel[1][2]);
               nzA.push_back(make_pair(3*i+2, 3*view1+0)); valsA.push_back(Rrel[2][0]);
               nzA.push_back(make_pair(3*i+2, 3*view1+1)); valsA.push_back(Rrel[2][1]);
               nzA.push_back(make_pair(3*i+2, 3*view1+2)); valsA.push_back(Rrel[2][2]);

               nzA.push_back(make_pair(3*i+0, 3*view2+0)); valsA.push_back(-1.0);
               nzA.push_back(make_pair(3*i+1, 3*view2+1)); valsA.push_back(-1.0);
               nzA.push_back(make_pair(3*i+2, 3*view2+2)); valsA.push_back(-1.0);
            } // end for (i)

            CCS_Matrix<double> A(3*nRelPoses, 3*nViews, nzA, valsA);

            if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
            {
#if defined(V3DLIB_ENABLE_ARPACK)
               Vector<double> sigma;
               Matrix<double> V;
               SparseSymmetricEigConfig cfg;
               cfg.maxArnoldiIterations = 100000;
               computeSparseSVD(A, V3D_ARPACK_SMALLEST_MAGNITUDE_EIGENVALUES, 3, sigma, V, cfg);
               //computeSparseSVD(A, V3D_ARPACK_SMALLEST_EIGENVALUES, 3, sigma, V, cfg);
               for (int i = 0; i < nViews; ++i)
               {
                  copyMatrixSlice(V, 3*i, 0, 3, 1, rotations[i], 0, 2);
                  copyMatrixSlice(V, 3*i, 1, 3, 1, rotations[i], 0, 1);
                  copyMatrixSlice(V, 3*i, 2, 3, 1, rotations[i], 0, 0);
               }
#endif
            }
            else
            {
               Matrix<double> AtA(3*nViews, 3*nViews);
               multiply_At_A_SparseDense(A, AtA);

               if (method == V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA)
               {
                  SVD<double> svd(AtA);
                  int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD
                  Matrix<double> const& V = svd.getV();
                  for (int i = 0; i < nViews; ++i)
                     copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
               }
               else
               {
                  Eigenvalue<double> svd(AtA);
                  int const startColumn = 0; // first columns of eigenvector matrix
                  Matrix<double> const& V = svd.getV();
                  for (int i = 0; i < nViews; ++i)
                     copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
               } // end if
            } // end if
            break;
         }
         default:
            throwV3DErrorHere("Unknown method argument for computeConsistentRotations().");
      } // end switch

      for (int i = 0; i < nViews; ++i)
         enforceRotationMatrix(rotations[i]);

      // Remove gauge freedom by setting R[0] = I.
      Matrix3x3d const R0t = rotations[0].transposed();
      for (int i = 0; i < nViews; ++i)
         rotations[i] = rotations[i] * R0t;

      // Note: it seems, that either all Rs have det(R)=1 or all have det(R)=-1.
      // Since we remove the gauge freedem by multiplying all rotations with R_0^t,
      // we always end up with det(R)=1 and any code to enforce a positive determinant
      // is not necessary.
   } // end computeConsistentRotations()