Eigen::Vector2f PhotoCamera::project(const Eigen::Vector3f &point) { Eigen::MatrixXf P = intrinsicMatrix*extrinsicMatrix.matrix(); Eigen::Vector4f pointH; pointH << point(0), point(1), point(2), 1; Eigen::Vector3f pixelH = P*pointH; return pixelH.hnormalized(); }