bool PinholeCamera<DISTORTION_T>::backProjectHomogeneousBatch( const Eigen::Matrix2Xd & imagePoints, Eigen::Matrix4Xd * directions, std::vector<bool> * success) const { const int numPoints = imagePoints.cols(); directions->row(3) = Eigen::VectorXd::Ones(numPoints); for (int i = 0; i < numPoints; ++i) { Eigen::Vector2d imagePoint = imagePoints.col(i); Eigen::Vector3d point; bool suc = backProject(imagePoint, &point); if(success) success->push_back(suc); directions->template block<3, 1>(0, i) = point; } return true; }