Пример #1
0
   void
   StdMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
                                           Matrix<double>& Bk,
                                           Matrix<double>& Ck,
                                           int i, int j, int k)
   {
      Vector3d XX;
      Matrix3x6d d_dRT;
      Matrix3x3d d_dX;
      this->poseDerivatives(i, j, XX, d_dRT, d_dX);

      double const f  = _cams[i].getFocalLength();
      double const ar = _cams[i].getAspectRatio();

      Matrix2x3d dp_dX;
      double const bx = f / (XX[2] * XX[2]);
      double const by = ar * bx;
      dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0;          dp_dX[0][2] = -bx * XX[0];
      dp_dX[1][0] = 0;          dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1];

      multiply_A_B(dp_dX, d_dRT, Ak);
      multiply_A_B(dp_dX, d_dX, Bk);
   } // end StdMetricBundleOptimizer::fillJacobians()
Пример #2
0
   void
   StereoMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak, Matrix<double>& Bk, Matrix<double>& Ck,
                                              int i, int j, int k)
   {
      int const l = _correspondingSubCamera[k];

      Vector3d XX;
      Matrix3x6d d_dRT;
      Matrix3x3d d_dX;

      ::poseDerivatives(_rotations[i], _translations[i], _rigCameras[l], _Xs[j], XX, d_dRT, d_dX);

      double const f  = _rigCameras[l].getFocalLength();
      double const ar = _rigCameras[l].getAspectRatio();

      Matrix2x3d dp_dX;
      double const bx = f / (XX[2] * XX[2]);
      double const by = ar * bx;
      dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0;          dp_dX[0][2] = -bx * XX[0];
      dp_dX[1][0] = 0;          dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1];

      multiply_A_B(dp_dX, d_dRT, Ak);
      multiply_A_B(dp_dX, d_dX, Bk);
   } // end StereoMetricBundleOptimizer::fillJacobians()
Пример #3
0
   void
   CommonInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
                                                       Matrix<double>& Bk,
                                                       Matrix<double>& Ck,
                                                       int i, int j, int k)
   {
      double const focalLength = _K[0][0];

      Vector3d XX;
      Matrix3x6d dXX_dRT;
      Matrix3x3d dXX_dX;
      this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX);

      Vector2d xu; // undistorted image point
      xu[0] = XX[0] / XX[2];
      xu[1] = XX[1] / XX[2];

      Vector2d const xd = _distortion(xu); // distorted image point

      Matrix2x2d dp_dxd;
      dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0;
      dp_dxd[1][0] = 0;           dp_dxd[1][1] = _cachedAspectRatio * focalLength;

      {
         // First, lets do the derivative wrt the structure and motion parameters.
         Matrix2x3d dxu_dXX;
         dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0;            dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]);
         dxu_dXX[1][0] = 0;            dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]);

         Matrix2x2d dxd_dxu = _distortion.derivativeWrtUndistortedPoint(xu);

         Matrix2x2d dp_dxu = dp_dxd * dxd_dxu;
         Matrix2x3d dp_dXX = dp_dxu * dxu_dXX;

         multiply_A_B(dp_dXX, dXX_dRT, Ak);
         multiply_A_B(dp_dXX, dXX_dX, Bk);
      } // end scope

      switch (_mode)
      {
         case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
         {
            // Focal length.
            Ck[0][0] = xd[0];
            Ck[1][0] = xd[1];

            // For radial, k1 only.
            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
            Ck[0][1] = d_dk1k2[0][0];
            Ck[1][1] = d_dk1k2[1][0];
            break;
         }
         case FULL_BUNDLE_FOCAL_AND_RADIAL:
         {
            // Focal length.
            Ck[0][0] = xd[0];
            Ck[1][0] = xd[1];

            // Radial k1 and k2.
            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
            copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 1);
            break;
         }
         case FULL_BUNDLE_RADIAL_TANGENTIAL:
         {
            Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu);
            Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2;
            copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ck, 0, 5);
            // No break here!
         }
         case FULL_BUNDLE_RADIAL:
         {
            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
            copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 3);
            // No break here!
         }
         case FULL_BUNDLE_FOCAL_LENGTH_PP:
         {
            Ck[0][1] = 1; Ck[0][2] = 0;
            Ck[1][1] = 0; Ck[1][2] = 1;
            // No break here!
         }
         case FULL_BUNDLE_FOCAL_LENGTH:
         {
            Ck[0][0] = xd[0];
            Ck[1][0] = xd[1];
         }
         case FULL_BUNDLE_METRIC:
         {
         }
      } // end switch
   } // end CommonInternalsMetricBundleOptimizer::fillJacobians()
Пример #4
0
   void
   VaryingInternalsMetricBundleOptimizer::fillJacobians(Matrix<double>& Ak,
                                                        Matrix<double>& Bk,
                                                        Matrix<double>& Ck,
                                                        int i, int j, int k)
   {
      Vector3d XX;
      Matrix3x6d dXX_dRT;
      Matrix3x3d dXX_dX;
      this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX);

      Vector2d xu; // undistorted image point
      xu[0] = XX[0] / XX[2];
      xu[1] = XX[1] / XX[2];

      Vector2d const xd = _distortions[i](xu); // distorted image point

      double const focalLength = _cams[i].getFocalLength();
      double const aspectRatio = _cams[i].getAspectRatio();

      Matrix2x2d dp_dxd;
      dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0;
      dp_dxd[1][0] = 0;           dp_dxd[1][1] = aspectRatio * focalLength;

      {
         // First, lets do the derivative wrt the structure and motion parameters.
         Matrix2x3d dxu_dXX;
         dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0;            dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]);
         dxu_dXX[1][0] = 0;            dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]);

         Matrix2x2d dxd_dxu = _distortions[i].derivativeWrtUndistortedPoint(xu);

         Matrix2x2d dp_dxu = dp_dxd * dxd_dxu;
         Matrix2x3d dp_dXX = dp_dxu * dxu_dXX;

         Matrix2x6d dp_dRT;

         multiply_A_B(dp_dXX, dXX_dRT, dp_dRT);
         copyMatrixSlice(dp_dRT, 0, 0, 2, 6, Ak, 0, 0);
         multiply_A_B(dp_dXX, dXX_dX, Bk);
      } // end scope

      switch (_mode)
      {
         case FULL_BUNDLE_RADIAL_TANGENTIAL:
         {
            Matrix2x2d dxd_dp1p2 = _distortions[i].derivativeWrtTangentialParameters(xu);
            Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2;
            copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ak, 0, 11);
            // No break here!
         }
         case FULL_BUNDLE_RADIAL:
         {
            Matrix2x2d dxd_dk1k2 = _distortions[i].derivativeWrtRadialParameters(xu);
            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
            copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ak, 0, 9);
            // No break here!
         }
         case FULL_BUNDLE_FOCAL_LENGTH_PP:
         {
            Ak[0][7] = 1; Ak[0][8] = 0;
            Ak[1][7] = 0; Ak[1][8] = 1;
            // No break here!
         }
         case FULL_BUNDLE_FOCAL_LENGTH:
         {
            Ak[0][6] = xd[0];
            Ak[1][6] = xd[1];
         }
         case FULL_BUNDLE_METRIC:
         {
         }
      } // end switch
   } // end VaryingInternalsMetricBundleOptimizer::fillJacobians()
Пример #5
0
   Matrix3x3d
   computeIntersectionCovariance(vector<Matrix3x4d> const& projections,
                                 vector<PointMeasurement> const& measurements,
                                 double sigma)
   {
      Matrix<double> Cp(3, 3, 0.0);
      Cp[0][0] = Cp[1][1] = sigma;
      Cp[2][2] = 0.0;

      int const N = measurements.size();

      Matrix<double> A(2*N, 4, 0.0);
      InlineMatrix<double, 2, 3> Sp;
      makeZeroMatrix(Sp);
      InlineMatrix<double, 2, 4> Sp_P;
      for (int i = 0; i < N; ++i)
      {
         Sp[0][1] = -1; Sp[0][2] = measurements[i].pos[1];
         Sp[1][0] =  1; Sp[1][2] = -measurements[i].pos[0];

         int const view = measurements[i].view;
         multiply_A_B(Sp, projections[view], Sp_P);

         A[2*i+0][0] = Sp_P[0][0]; A[2*i+0][1] = Sp_P[0][1]; A[2*i+0][2] = Sp_P[0][2]; A[2*i+0][3] = Sp_P[0][3];
         A[2*i+1][0] = Sp_P[1][0]; A[2*i+1][1] = Sp_P[1][1]; A[2*i+1][2] = Sp_P[1][2]; A[2*i+1][3] = Sp_P[1][3];
      } // end for (i)

      SVD<double> svd(A);

      Matrix<double> V;
      svd.getV(V);

      Vector4d X;
      X[0] = V[0][3]; X[1] = V[1][3]; X[2] = V[2][3]; X[3] = V[3][3];

      Vector3d P;
      Matrix<double> S(2, 3, 0.0);
      Matrix<double> B(2*N, 3*N, 0.0);

      for (int i = 0; i < N; ++i)
      {
         int const view = measurements[i].view;
         multiply_A_v(projections[view], X, P);
         P[0] /= P[2]; P[1] /= P[2]; P[2] = 1.0;

         S[0][1] = -P[2]; S[0][2] =  P[1];
         S[1][0] =  P[2]; S[1][2] = -P[0];

         B[2*i+0][3*i+0] = -S[0][0]; B[2*i+0][3*i+1] = -S[0][1]; B[2*i+0][3*i+2] = S[0][2];
         B[2*i+1][3*i+0] = -S[1][0]; B[2*i+1][3*i+1] = -S[1][1]; B[2*i+1][3*i+2] = S[1][2];
      } // end for (i)

      Matrix<double> C(3*N, 3*N, 0.0);
      for (int i = 0; i < N; ++i)
      {
         C[3*i+0][3*i+0] = Cp[0][0]; C[3*i+0][3*i+1] = Cp[0][1]; C[3*i+0][3*i+2] = Cp[0][2];
         C[3*i+1][3*i+0] = Cp[1][0]; C[3*i+1][3*i+1] = Cp[1][1]; C[3*i+1][3*i+2] = Cp[1][2];
         C[3*i+2][3*i+0] = Cp[2][0]; C[3*i+2][3*i+1] = Cp[2][1]; C[3*i+2][3*i+2] = Cp[2][2];
      } // end for (i)

      Matrix<double> B_C(2*N, 3*N);
      multiply_A_B(B, C, B_C);
      Matrix<double> T(2*N, 2*N);
      multiply_A_Bt(B_C, B, T);

      Matrix<double> Tinv;
      invertMatrix(T, Tinv);

      Matrix<double> NN(5, 5), N4(4, 4);
      Matrix<double> At_Tinv(4, 2*N);
      multiply_At_B(A, Tinv, At_Tinv);
      multiply_A_B(At_Tinv, A, N4);

      for (int r = 0; r < 4; ++r)
         for (int c = 0; c < 4; ++c)
            NN[r][c] = N4[r][c];

      NN[0][4] = NN[4][0] = X[0];
      NN[1][4] = NN[4][1] = X[1];
      NN[2][4] = NN[4][2] = X[2];
      NN[3][4] = NN[4][3] = X[3];
      NN[4][4] = 0.0;

      Matrix<double> Ninv(5, 5);
      invertMatrix(NN, Ninv);

      Matrix4x4d sigma_XX;
      for (int r = 0; r < 4; ++r)
         for (int c = 0; c < 4; ++c)
            sigma_XX[r][c] = Ninv[r][c];

      Matrix3x4d Je;
      makeZeroMatrix(Je);
      Je[0][0] = Je[1][1] = Je[2][2] = 1.0 / X[3];
      Je[0][3] = -X[0] / (X[3]*X[3]);
      Je[1][3] = -X[1] / (X[3]*X[3]);
      Je[2][3] = -X[2] / (X[3]*X[3]);

      Matrix3x3d sigma_X = Je * sigma_XX * Je.transposed();
      return sigma_X;
   }