DepthImageUtils.h

Go to the documentation of this file.

#pragma once
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <ArmarXCore/interface/core/SharedMemory.h>
 
#include <VisionX/components/pointcloud_core/PCLUtilities.h>
#include <VisionX/tools/ImageUtil.h>
 
#include <Image/ByteImage.h>
 
namespace armarx
{
    class DepthImageUtils
    {
 
        typedef pcl::PointXYZRGBL PointL;
 
    public:
        DepthImageUtils(float fovH = 43.0f, float fovV = 57.0f);
 
        void setFieldOfView(float fovH, float fovV);
 
        void updateCameraParameters(float fx, float fy, int width, int height);
 
        void convertDepthImageToPointCloud(CByteImage** images,
                                           armarx::MetaInfoSizeBasePtr imageMetaInfo,
                                           size_t numImages,
                                           pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& result);
 
        template <typename PointCloudPtrT>
        void
        convertDepthImageToPointCloud(CByteImage** images,
                                      armarx::MetaInfoSizeBasePtr imageMetaInfo,
                                      PointCloudPtrT& result)
        {
            using PointT = typename PointCloudPtrT::PointType;
 
            result->width = images[0]->width;
            result->height = images[0]->height;
            result->header.stamp = imageMetaInfo->timeProvided;
            result->points.resize(images[0]->width * images[0]->height);
            result->is_dense = true;
 
 
            const size_t width = static_cast<size_t>(images[0]->width);
            const size_t height = static_cast<size_t>(images[0]->height);
            const float halfWidth = (width / 2.0);
            const float halfHeight = (height / 2.0);
            const auto& image0Data = images[0]->pixels;
            const auto& image1Data = images[1]->pixels;
 
            for (size_t j = 0; j < height; j++)
            {
                for (size_t i = 0; i < width; i++)
                {
                    auto coord = j * width + i;
                    //            unsigned short value = images[1]->pixels[3 * coord + 0]
                    //                                   + (images[1]->pixels[3 * coord + 1] << 8);
                    auto value = visionx::tools::rgbToDepthValue(image1Data[3 * coord + 0],
                                                                 image1Data[3 * coord + 1],
                                                                 image1Data[3 * coord + 2]);
 
                    PointT& p = result->points.at(coord);
                    auto index = 3 * (coord);
                    p.r = image0Data[index + 0];
                    p.g = image0Data[index + 1];
                    p.b = image0Data[index + 2];
 
                    if (value == 0)
                    {
                        p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
                    }
                    else
                    {
                        p.z = static_cast<float>(value);
                        //p.z /= 1000.0;
                        p.x = -1.0 * (i - halfWidth) / width * p.z * scaleX;
                        p.y = (halfHeight - j) / height * p.z * scaleY;
                    }
                }
            }
        }
 
    private:
        float scaleX;
        float scaleY;
    };
} // namespace armarx