projection.cc

Go to the documentation of this file.

// Copyright (c) 2007, 2008 libmv authors.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
 
#include "libmv/multiview/projection.h"
#include "libmv/numeric/numeric.h"
 
namespace libmv {
 
void P_From_KRt(const Mat3& K, const Mat3& R, const Vec3& t, Mat34* P) {
  P->block<3, 3>(0, 0) = R;
  P->col(3) = t;
  (*P) = K * (*P);
}
 
void KRt_From_P(const Mat34& P, Mat3* Kp, Mat3* Rp, Vec3* tp) {
  // Decompose using the RQ decomposition HZ A4.1.1 pag.579.
  Mat3 K = P.block(0, 0, 3, 3);
 
  Mat3 Q;
  Q.setIdentity();
 
  // Set K(2,1) to zero.
  if (K(2, 1) != 0) {
    double c = -K(2, 2);
    double s = K(2, 1);
    double l = sqrt(c * c + s * s);
    c /= l;
    s /= l;
    Mat3 Qx;
    // clang-format off
    Qx << 1, 0,  0,
          0, c, -s,
          0, s,  c;
    // clang-format on
    K = K * Qx;
    Q = Qx.transpose() * Q;
  }
  // Set K(2,0) to zero.
  if (K(2, 0) != 0) {
    double c = K(2, 2);
    double s = K(2, 0);
    double l = sqrt(c * c + s * s);
    c /= l;
    s /= l;
    Mat3 Qy;
    // clang-format off
    Qy << c, 0, s,
          0, 1, 0,
         -s, 0, c;
    // clang-format on
    K = K * Qy;
    Q = Qy.transpose() * Q;
  }
  // Set K(1,0) to zero.
  if (K(1, 0) != 0) {
    double c = -K(1, 1);
    double s = K(1, 0);
    double l = sqrt(c * c + s * s);
    c /= l;
    s /= l;
    Mat3 Qz;
    // clang-format off
    Qz << c, -s,  0,
          s,  c,  0,
          0,  0,  1;
    // clang-format on
    K = K * Qz;
    Q = Qz.transpose() * Q;
  }
 
  Mat3 R = Q;
 
  // Ensure that the diagonal is positive.
  // TODO(pau) Change this to ensure that:
  //  - K(0,0) > 0
  //  - K(2,2) = 1
  //  - det(R) = 1
  if (K(2, 2) < 0) {
    K = -K;
    R = -R;
  }
  if (K(1, 1) < 0) {
    Mat3 S;
    // clang-format off
    S << 1,  0,  0,
         0, -1,  0,
         0,  0,  1;
    // clang-format on
    K = K * S;
    R = S * R;
  }
  if (K(0, 0) < 0) {
    Mat3 S;
    // clang-format off
    S << -1, 0, 0,
          0, 1, 0,
          0, 0, 1;
    // clang-format on
    K = K * S;
    R = S * R;
  }
 
  // Compute translation.
  Vec p(3);
  p << P(0, 3), P(1, 3), P(2, 3);
  // TODO(pau) This sould be done by a SolveLinearSystem(A, b, &x) call.
  // TODO(keir) use the eigen LU solver syntax...
  Vec3 t = K.inverse() * p;
 
  // scale K so that K(2,2) = 1
  K = K / K(2, 2);
 
  *Kp = K;
  *Rp = R;
  *tp = t;
}
 
void ProjectionShiftPrincipalPoint(const Mat34& P,
                                   const Vec2& principal_point,
                                   const Vec2& principal_point_new,
                                   Mat34* P_new) {
  Mat3 T;
  // clang-format off
  T << 1, 0, principal_point_new(0) - principal_point(0),
       0, 1, principal_point_new(1) - principal_point(1),
       0, 0,                                           1;
  // clang-format on
  *P_new = T * P;
}
 
void ProjectionChangeAspectRatio(const Mat34& P,
                                 const Vec2& principal_point,
                                 double aspect_ratio,
                                 double aspect_ratio_new,
                                 Mat34* P_new) {
  Mat3 T;
  // clang-format off
  T << 1,                               0, 0,
       0, aspect_ratio_new / aspect_ratio, 0,
       0,                               0, 1;
  // clang-format on
  Mat34 P_temp;
 
  ProjectionShiftPrincipalPoint(P, principal_point, Vec2(0, 0), &P_temp);
  P_temp = T * P_temp;
  ProjectionShiftPrincipalPoint(P_temp, Vec2(0, 0), principal_point, P_new);
}
 
void HomogeneousToEuclidean(const Mat& H, Mat* X) {
  int d = H.rows() - 1;
  int n = H.cols();
  X->resize(d, n);
  for (size_t i = 0; i < n; ++i) {
    double h = H(d, i);
    for (int j = 0; j < d; ++j) {
      (*X)(j, i) = H(j, i) / h;
    }
  }
}
 
void HomogeneousToEuclidean(const Mat3X& h, Mat2X* e) {
  e->resize(2, h.cols());
  e->row(0) = h.row(0).array() / h.row(2).array();
  e->row(1) = h.row(1).array() / h.row(2).array();
}
void HomogeneousToEuclidean(const Mat4X& h, Mat3X* e) {
  e->resize(3, h.cols());
  e->row(0) = h.row(0).array() / h.row(3).array();
  e->row(1) = h.row(1).array() / h.row(3).array();
  e->row(2) = h.row(2).array() / h.row(3).array();
}
 
void HomogeneousToEuclidean(const Vec3& H, Vec2* X) {
  double w = H(2);
  *X << H(0) / w, H(1) / w;
}
 
void HomogeneousToEuclidean(const Vec4& H, Vec3* X) {
  double w = H(3);
  *X << H(0) / w, H(1) / w, H(2) / w;
}
 
void EuclideanToHomogeneous(const Mat& X, Mat* H) {
  int d = X.rows();
  int n = X.cols();
  H->resize(d + 1, n);
  H->block(0, 0, d, n) = X;
  H->row(d).setOnes();
}
 
void EuclideanToHomogeneous(const Vec2& X, Vec3* H) {
  *H << X(0), X(1), 1;
}
 
void EuclideanToHomogeneous(const Vec3& X, Vec4* H) {
  *H << X(0), X(1), X(2), 1;
}
 
// TODO(julien) Call conditioning.h/ApplyTransformationToPoints ?
void EuclideanToNormalizedCamera(const Mat2X& x, const Mat3& K, Mat2X* n) {
  Mat3X x_image_h;
  EuclideanToHomogeneous(x, &x_image_h);
  Mat3X x_camera_h = K.inverse() * x_image_h;
  HomogeneousToEuclidean(x_camera_h, n);
}
 
void HomogeneousToNormalizedCamera(const Mat3X& x, const Mat3& K, Mat2X* n) {
  Mat3X x_camera_h = K.inverse() * x;
  HomogeneousToEuclidean(x_camera_h, n);
}
 
double Depth(const Mat3& R, const Vec3& t, const Vec3& X) {
  return (R * X)(2) + t(2);
}
 
double Depth(const Mat3& R, const Vec3& t, const Vec4& X) {
  Vec3 Xe = X.head<3>() / X(3);
  return Depth(R, t, Xe);
}
 
}  // namespace libmv