LaserScanAggregator.cpp

Go to the documentation of this file.

#include "LaserScanAggregator.h"
 
#include <vector>
 
#include <pcl/common/transforms.h>
 
#include "RobotAPI/libraries/armem_vision/types.h"
 
#include "armarx/localization_and_mapping/cartographer_adapter/util/laser_scanner_conversion.h"
 
namespace armarx
{
    class GroudPlaneProjector
    {
    public:
        void
        project(pcl::PointCloud<pcl::PointXYZ>& cloud)
        {
            std::for_each(cloud.points.begin(), cloud.points.end(), [](auto& pt) { pt.z = 0.F; });
        }
    };
 
    LaserScanAggregator::LaserScanAggregator(
        const LaserScanAggregator::SensorPoseMap& sensorPoses,
        const LaserScanAggregator::RobotPoseQueue& robotPoseQueue,
        const float minRange) :
        sensorPoses(sensorPoses),
        robotPoseQueue(robotPoseQueue),
        rangeFilter(minRange),
        cloud(new PointCloud)
    {
    }
 
    void
    LaserScanAggregator::add(const armem::LaserScanStamped& scan)
    {
        const auto& robot_T_sensor = sensorPoses.at(scan.header.frame);
 
        const auto& timestamp = scan.header.timestamp;
 
        if (not robotPoseQueue.has(timestamp.toMicroSeconds()))
        {
            ARMARX_INFO << "Cannot lookup pose for timestamp " << timestamp;
            return;
        }
 
        const Eigen::Isometry3f map_T_robot =
            robotPoseQueue.lookupInterpolate(timestamp.toMicroSeconds())->pose;
        const Eigen::Isometry3f map_T_sensor = map_T_robot * robot_T_sensor;
 
        auto cloudSensorFrame = toCartesian<Point>(scan.data);
 
        // actually, would be more efficient to integrate filtering into toCartesian ...
        rangeFilter.applyFilter(cloudSensorFrame);
 
        PointCloud cloudMapFrame;
        pcl::transformPointCloud(cloudSensorFrame, cloudMapFrame, map_T_sensor);
 
        GroudPlaneProjector projector;
        projector.project(cloudMapFrame);
 
        *cloud += cloudMapFrame;
    }
 
    void
    LaserScanAggregator::add(const std::vector<armem::LaserScanStamped>& scans)
    {
        std::for_each(
            scans.begin(), scans.end(), [&](const armem::LaserScanStamped& scan) { add(scan); });
    }
 
    const LaserScanAggregator::PointCloud::Ptr&
    LaserScanAggregator::pointCloud() const
    {
        return cloud;
    }
 
 
} // namespace armarx