point_cloud.h

Go to the documentation of this file.

#pragma once
 
#include <map>
 
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <VirtualRobot/math/Helpers.h>
 
#include <VisionX/libraries/VoxelGridCore/VoxelGrid.hpp>
 
#include "../types.h"
 
namespace visionx::voxelgrid::utils
{
 
    /**
     * @brief Transform a point cloud to local grid coordinates.
     * @param pointCloud The point cloud.
     * @param pointCloudPose The global pose of the point cloud.
     * @param gridPose The global pose of the grid.
     * @return The point cloud in grid's local coordinates.
     */
    template <class PointT>
    pcl::PointCloud<PointT> transformPointCloudToGrid(const pcl::PointCloud<PointT>& pointCloud,
                                                      const Eigen::Matrix4f& pointCloudPose,
                                                      const Eigen::Matrix4f& gridPose);
 
    template <class PointT>
    pcl::PointCloud<PointT>
    transformPointCloudToGrid(const pcl::PointCloud<PointT>& pointCloud,
                              const Eigen::Matrix4f& pointCloudPose,
                              const Eigen::Matrix4f& gridPose)
    {
        // p_w = pose_pc  * p_pc
        // p_w = pose_gr  * p_gr
 
        // wanted: tf with:  p_gr = tf * p_pc
        // p_gr = (pose_gr)^-1 * p_w
        // p_gr = (pose_gr)^-1 * pose_pc * p_pc
        // => tf = (pose_gr)^-1 * pose_pc
 
        const Eigen::Matrix4f gridPoseInv = math::Helpers::InvertedPose(gridPose);
 
        const Eigen::Matrix4f tfPointCloud2Grid = gridPoseInv * pointCloudPose;
 
        pcl::PointCloud<PointT> transformed;
        pcl::transformPointCloud(pointCloud, transformed, tfPointCloud2Grid);
 
        return transformed;
    }
 
 
} // namespace visionx::voxelgrid::utils