Use Eigen::Isometry3d as much as we can (rather than Eigen::Matrix4d) for transformation representation.