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;
}
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;
}