I have a point cloud of a scene in which one object of interest is present along with some other points. I also know the coordinates of the eight corners of the 3D bounding box which happens to be a cuboid.
Now I have a use case where I need to transform this point cloud into the camera coordinate system where each 3D point will get a new XYZ coordinate. Similarly, the eight corner points of the 3D bounding box should be transformed into the camera coordinate system.
Now I have the extrinsic matrix; (rotation matrix and translation) which converts the column vectors from the camera coordinate system to the world coordinate system. I convert the rotation matrix into the quaternion for simplicity.
I want to convert the point cloud and bounding box coordinates from world coordinate system to the camera coordinate system. So I obtain the inverse transformation by simply taking the inverse rotation and negative translation as follows:
void get_inverse_transform( const Eigen::Quaternionf& orientation,
const Eigen::Vector3f& translation,
Eigen::Quaternionf& inverse_orientation,
Eigen::Vector3f& inverse_traslation)
{
inverse_orientation = orientation.normalize().inverse().normalise();
inverse_translation = -1*inverse_orientation._transformVector(position);
}
Now, inverse_translation
and inverse_orientation
holds the transformation which will convert the points in world coordinate system to the camera coordinate system.
So, I transform the eight corners of the bounding box using this transformation into camera coordinates in the following way:
void project2CS( const Eigen::Quaternionf& inverse_orientation,
const Eigen::Vector3f& inverse_position,
std::vector<Eigen::Vector3f>& bbox3d_corners)
{
for( auto& corner: bbox3d_corners )
{
auto transformed_vec = inverse_orientation._transformVector(corner) + inverse_translation;
corner[0] = transformed_vec[0];
corner[1] = transformed_vec[1];
corner[2] = transformed_vec[2];
}
}
I also transform the point cloud into the camera coordinate system and visualize it with the bounding box, the bounding box does not surround the object anymore. Moreover, I see the size of the bounding box change slightly.
I suspect I am doing something wrong in getting the inverse transformation. Any ideas on how to get this work?