FramedPointCloud.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.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::RobotPtr& 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::RobotPtr& robot) const
        {
            return getFramedPose(robot->getName());
        }
 
        /// Get the current global pose.
        Eigen::Matrix4f getGlobalPose(const VirtualRobot::RobotPtr& 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::RobotPtr& 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>;
 
}
 
 
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::RobotPtr& 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);
    }
}