DepthImageUtils.cpp

Go to the documentation of this file.

#include "DepthImageUtils.h"
 
#include <cmath>
 
#include <Image/ByteImage.h>
#include <Image/FloatImage.h>
 
 
using namespace armarx;
 
DepthImageUtils::DepthImageUtils(float fovH, float fovV)
{
    setFieldOfView(fovH, fovV);
}
 
void
DepthImageUtils::setFieldOfView(float fovH, float fovV)
{
    scaleX = std::tan(fovH * M_PI / 180.0 / 2.0) * 2.0;
    scaleY = std::tan(fovV * M_PI / 180.0 / 2.0) * 2.0;
}
 
void
DepthImageUtils::updateCameraParameters(float fx, float fy, int width, int height)
{
    const float fovH = 180.0 / M_PI * 2.0 * std::atan(width / (2.0 * fx));
    const float fovV = 180.0 / M_PI * 2.0 * std::atan(height / (2.0 * fy));
    setFieldOfView(fovH, fovV);
}
 
void
DepthImageUtils::convertDepthImageToPointCloud(CByteImage** images,
                                               armarx::MetaInfoSizeBasePtr imageMetaInfo,
                                               size_t numImages,
                                               pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& result)
{
 
    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]);
 
            PointL& 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;
            }
 
            if (numImages > 2)
            {
                auto& image2Data = images[2]->pixels;
 
                if (images[2]->bytesPerPixel == 3)
                {
                    p.label = static_cast<unsigned int>(image2Data[index + 0] +
                                                        (image2Data[index + 1] << 8) +
                                                        (image2Data[index + 2] << 16));
                }
                else
                {
                    p.label = static_cast<unsigned int>(image2Data[coord]);
                }
            }
            else
            {
                p.label = 0;
            }
        }
    }
}