Revision history [back]

It turns out the "P" matrix in the camera_info message does this more or less directly. I was able to do the needed transformation this way:

From the camera_info callback

m_worldToCamera = Eigen::Matrix<double,3,4,Eigen::RowMajor>( msg->P.data() );

And then later

Eigen::Vector3d imgPoint;
cameraPoint = m_worldToCamera * Eigen::Vector4d( point.x(), point.y(), point.z(), 1 );

Eigen::Vector2d flatPoint;
flatPoint.x() = cameraPoint.x() / cameraPoint.z();
flatPoint.y() = cameraPoint.y() / cameraPoint.z();