Verified code comparing CISST vs ROS pose