bool does_matrix_contain_nan(const Eigen::Matrix3f &m)
 {
   for (int i=0;i<m.matrix().rows();i++) {
     for (int j=0;j<m.matrix().cols();j++) {
       if ((boost::math::isnan)(m(i,j))) return true;
     }
   }
 return false;
 }
Exemple #2
0
bool Utils::
composeViewMatrix(Eigen::Projective3f& oMatrix, const Eigen::Matrix3f& iCalib,
                  const Eigen::Isometry3f& iPose, const bool iIsOrthographic) {
  Eigen::Projective3f calib = Eigen::Projective3f::Identity();
  calib.matrix().col(2).swap(calib.matrix().col(3));
  calib.matrix().topLeftCorner<2,3>() = iCalib.matrix().topRows<2>();
  calib.matrix().bottomLeftCorner<1,3>() = iCalib.matrix().bottomRows<1>();
  if (iIsOrthographic) calib.matrix().col(2).swap(calib.matrix().col(3));
  oMatrix = calib*iPose.inverse();
  return true;
}