Upstream #1361 commit by commit after discussions

Hi, while profiling some code I found out that the creation of the PointCloud structure takes too much time. Digging in I found out that the problem is the std::vector for the points. For some reason std::vector and Eigen don't work that well together.

I did a simple code to show this issue:

Updated with real case

PointCloud Resize: 0.086795s
PointCloud Reserve: 0.132422s
Array: 0.0678629s

A simple array is 30% faster than the PointCloud. Would it be crazy to remove std::vector from the PointCloud structure?

Updated with real case

#include <iostream>
#include <chrono>
#include <array>

#include <pcl/common/common.h>

const int width = 1920, height = 1080;
const int cycles = 1000;
const int cx = width/2, cy = height/2;
const double zeroPlanePixelSize = 0.10520000010728836;
const int zeroPlaneDistance = 120;

std::array<double, 5000> zFactors;

void pointCloudResize(uint8_t* rgb, uint16_t* depthMap)
{
    auto total = std::chrono::duration<double>::zero();
    for (int c = 0; c < cycles; c++) {
        auto start = std::chrono::high_resolution_clock::now();

        {
            std::vector<pcl::PointXYZRGBA> points;
            points.resize(width*height);

            for(int y=0, i=0;y<height;y++){
                for(int x=0;x<width;x++,i++) {
                    pcl::PointXYZRGBA& point = points[i];

                    point.x = NAN;
                    point.y = NAN;
                    point.z = NAN;

                    point.r = rgb[3*i];
                    point.g = rgb[3*i+1];
                    point.b = rgb[3*i+2];

                    point.a = 255;

                    uint16_t depth = depthMap[i];
                    if (depth != 0) {
                        double zFactor = zFactors[depth];

                        point.x = static_cast<float>((x - cx) * zFactor * 0.001);
                        point.y = static_cast<float>((y - cy) * zFactor * 0.001);
                        point.z = static_cast<float>(depth * 0.001);

                        point.a = 0;
                    }
                }
            }
        }

        total += std::chrono::high_resolution_clock::now() - start;
    }
    std::cout << "PointCloud Resize: " << total.count() / cycles << "s" << std::endl;
}

void pointCloudReserve(uint8_t* rgb, uint16_t* depthMap)
{
    auto total = std::chrono::duration<double>::zero();
    for (int c = 0; c < cycles; c++) {
        auto start = std::chrono::high_resolution_clock::now();

        {
            std::vector<pcl::PointXYZRGBA> points;
            points.reserve(width*height);

            for(int y=0, i=0;y<height;y++){
                for(int x=0;x<width;x++,i++) {
                    pcl::PointXYZRGBA point;

                    point.x = NAN;
                    point.y = NAN;
                    point.z = NAN;

                    point.r = rgb[3*i];
                    point.g = rgb[3*i+1];
                    point.b = rgb[3*i+2];

                    point.a = 255;

                    uint16_t depth = depthMap[i];
                    if (depth != 0) {
                        double zFactor = zFactors[depth];

                        point.x = static_cast<float>((x - cx) * zFactor * 0.001);
                        point.y = static_cast<float>((y - cy) * zFactor * 0.001);
                        point.z = static_cast<float>(depth * 0.001);

                        point.a = 0;
                    }

                    points.push_back(point);
                }
            }
        }

        total += std::chrono::high_resolution_clock::now() - start;
    }
    std::cout << "PointCloud Reserve: " << total.count() / cycles << "s" << std::endl;
}

void array(uint8_t* rgb, uint16_t* depthMap)
{
    auto total = std::chrono::duration<double>::zero();
    for (int c = 0; c < cycles; c++) {
        auto start = std::chrono::high_resolution_clock::now();

        {
            auto points = new pcl::PointXYZRGBA[width * height];

            for(int y=0, i=0;y<height;y++){
                for(int x=0;x<width;x++,i++) {
                    pcl::PointXYZRGBA& point = points[i];

                    point.x = NAN;
                    point.y = NAN;
                    point.z = NAN;

                    point.r = rgb[3*i];
                    point.g = rgb[3*i+1];
                    point.b = rgb[3*i+2];

                    point.a = 255;

                    uint16_t depth = depthMap[i];
                    if (depth != 0) {
                        double zFactor = zFactors[depth];

                        point.x = static_cast<float>((x - cx) * zFactor * 0.001);
                        point.y = static_cast<float>((y - cy) * zFactor * 0.001);
                        point.z = static_cast<float>(depth * 0.001);

                        point.a = 0;
                    }
                }
            }

            delete[] points;
        }

        total += std::chrono::high_resolution_clock::now() - start;
    }
    std::cout << "Array: " << total.count() / cycles << "s" << std::endl;
}

int main(int argc, const char * argv[]) {
    for(int depth=0;depth<zFactors.size();depth++){
        zFactors[depth] = 2 * depth * zeroPlanePixelSize / zeroPlaneDistance;
    }

    uint8_t * rgb = new uint8_t[width*height*3];
    uint16_t * depthMap = new uint16_t[width*height];

    for(int i=0;i<width*height;i++){
        depthMap[i] = static_cast<uint16_t>((5000*i)/(width*height));
    }

    pointCloudResize(rgb, depthMap);
    pointCloudReserve(rgb, depthMap);
    array(rgb, depthMap);

    delete [] rgb;
    delete [] depthMap;

    return 0;
}