FramedPointCloud.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
 
#include <VirtualRobot/Robot.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace visionx
{
 
    /**
     * @brief Transform a point cloud from a robot frame to another.
     * @param sourceCloud The source point cloud.
     * @param targetCloud The target point cloud (may be same as `sourceCloud`).
     * @param sourceFrame The current frame of `sourceCloud`.
     * @param targetFrame The current frame to transform to.
     * @param robot The reference robot to use for transformation.
     */
    template <class PointT>
    void transformPointCloudFromTo(const pcl::PointCloud<PointT>& sourceCloud,
                                   pcl::PointCloud<PointT>& targetCloud,
                                   const std::string& sourceFrame,
                                   const std::string& targetFrame,
                                   const VirtualRobot::RobotConstPtr& robot);
 
    /**
     * @brief A point cloud which keeps track of its reference coordinate frame
     * and allows changing frames using `armarx::FramedPose`.
     */
    template <class _PointT>
    class FramedPointCloud
    {
    public:
        using PointT = _PointT;
        using PointCloudT = pcl::PointCloud<PointT>;
        using PointCloudPtrT = typename PointCloudT::Ptr;
 
    public:
        /// Construct an empty point cloud in global frame.
        FramedPointCloud() = default;
 
        /// Construct an empty point cloud residing in given frame.
        FramedPointCloud(const std::string& frameName) : frame(frameName)
        {
        }
 
        /// Construct a framed point cloud managing given point cloud in global frame.
        FramedPointCloud(PointCloudPtrT pointCloudPtr) : cloud(pointCloudPtr)
        {
        }
 
        /// Construct a framed point cloud managing given point cloud residing in given frame.
        FramedPointCloud(PointCloudPtrT pointCloudPtr, const std::string& frameName) :
            cloud(pointCloudPtr), frame(frameName)
        {
        }
 
        /// Get the current frame.
        std::string
        getFrame() const
        {
            return frame;
        }
 
        void
        setFrameWithoutTransformation(const std::string& f)
        {
            frame = f;
        }
 
        /// Get the current pose as `FramedPose`.
        armarx::FramedPose
        getFramedPose(const std::string& agentName) const
        {
            return armarx::FramedPose(Eigen::Matrix4f::Identity(), frame, agentName);
        }
 
        armarx::FramedPose
        getFramedPose(const VirtualRobot::RobotConstPtr& robot) const
        {
            return getFramedPose(robot->getName());
        }
 
        /// Get the current global pose.
        Eigen::Matrix4f
        getGlobalPose(const VirtualRobot::RobotConstPtr& robot) const
        {
            return getFramedPose(robot).toGlobalEigen(robot);
        }
 
        /// Transform the point cloud from the current frame to the new frame using the given reference robot.
        void
        changeFrame(const std::string& newFrame, const VirtualRobot::RobotConstPtr& robot)
        {
            transformPointCloudFromTo<PointT>(*cloud, *cloud, frame, newFrame, robot);
            this->frame = newFrame;
        }
 
        void
        transformBy(const Eigen::Matrix4f& transform)
        {
            pcl::transformPointCloud(*cloud, *cloud, transform);
        }
 
    public:
        /// The point cloud.
        PointCloudPtrT cloud{new PointCloudT()};
 
 
    private:
        /// The current frame.
        std::string frame = armarx::GlobalFrame;
    };
 
 
    // Template deduction guidelines for PointT from PointCloudPtrT.
    template <class PointCloudPtrT>
    FramedPointCloud(PointCloudPtrT pointCloudPtr)
        -> FramedPointCloud<typename PointCloudPtrT::element_type::PointType>;
 
    template <class PointCloudPtrT>
    FramedPointCloud(PointCloudPtrT pointCloudPtr, const std::string& frameName)
        -> FramedPointCloud<typename PointCloudPtrT::element_type::PointType>;
 
} // namespace visionx
 
template <class PointT>
void
visionx::transformPointCloudFromTo(const pcl::PointCloud<PointT>& sourceCloud,
                                   pcl::PointCloud<PointT>& targetCloud,
                                   const std::string& sourceFrame,
                                   const std::string& targetFrame,
                                   const VirtualRobot::RobotConstPtr& robot)
{
    if (sourceFrame != targetFrame)
    {
        armarx::FramedPose framedPose(Eigen::Matrix4f::Identity(), sourceFrame, robot->getName());
        framedPose.changeFrame(robot, targetFrame);
        const Eigen::Matrix4f sourceToTarget = framedPose.toEigen();
        pcl::transformPointCloud(sourceCloud, targetCloud, sourceToTarget);
    }
}