Параллельный выбор с сохранением порядка из массива с использованием tbb

У меня есть диапазон-изображение, и я хочу преобразовать его в облако точек libpointmatcher. Облако представляет собой Eigen::Matrix с 4 строками (x, y, z, 1) и несколькими столбцами для каждой точки. Изображение диапазона - это unsigned short*array, включающий значения диапазона (z) и unsigned char*array, включая информацию о видимости пикселей.

Последовательно мой код выглядит так:

//container to hold the data
std::vector<Eigen::Vector4d> vec;
vec.reserve(this->Height*this->Width);

//contains information about pixel visibility
unsigned char* mask_data = (unsigned char*)range_image.mask.ToPointer();
//contains the actual pixel data 
unsigned short* pixel_data = (unsigned short*)range_image.pixel.ToPointer();

for (int y =0;y < range_image.Height; y++)
{ 
    for (int x = 0; x < range_image.Width; x++)
    {   
        int index  =x+y*range_image.Width;
        if(*(mask_data+index) != 0)
        {               
            vec.push_back(Eigen::Vector4d(x,y,(double)*(data+index),1));
        }               
    }
}
// libpointmatcher point cloud with size of visible pixel
PM::Matrix features(4,vec.size());
PM::DataPoints::Labels featureLabels;
featureLabels.resize(4);
featureLabels[0] =  PM::DataPoints::Label::Label("x");
featureLabels[1] =  PM::DataPoints::Label::Label("y");
featureLabels[2] =  PM::DataPoints::Label::Label("z");
featureLabels[3] =  PM::DataPoints::Label::Label("pad");

//fill with data
for(int i = 0; i<vec.size(); i++)
{
    features.col(i) = vec[i];
}   

Из-за больших изображений этот цикл занимает 500 мс для 840000 точек, и это слишком медленно. Теперь моя идея заключалась в том, чтобы объединить приведенный выше код в одну параллельную функцию. Проблема в том, что Eigen::Matrix не предоставляет функциональных возможностей push_back, я не знаю заранее количество видимых точек и мне нужны точки в правильном порядке для обработки облака точек.

Поэтому мне нужен параллельный алгоритм для извлечения видимых 3D-точек из моего изображения диапазона и вставки их в Eigen :: Matrix в правильном порядке. Я работаю с Microsoft Visual Studio 2012 и могу использовать либо OpenMP 2.0, либо TBB. Я ценю любую помощь :)

ОБНОВЛЕНИЕ

Как подсказал Арч Д. Робисон, я попробовал tbb::parallel_scan. Я передал массив масок и двойной массив для хранения трехмерных координат. Выходной массив в четыре раза превышает размер входного массива для хранения однородных трехмерных данных (x, y, z, 1). Затем я отображаю массив otput в Eigen :: Matrix. Количество строк фиксировано, а столбцы появляются из результата parallel_scan.

size_t vec_size = width*height;
double* out = new double[vec_size * 4];
size_t m1 = Compress(mask, pixel, out, height, width,
 [](unsigned char x)  {return x != 0; });
Map<MatrixXd> features(out, 4, m1);

. Вот код из operator():

void operator()(const tbb::blocked_range2d<size_t, size_t>& r, Tag) {
    // Use local variables instead of member fields inside the loop,
    // to improve odds that values will be kept in registers.
    size_t j = sum;
    const unsigned char* m = in;
    const unsigned short* p = in2;
    T* values = out;
    size_t yend = r.rows().end();
    for (size_t y = r.rows().begin(); y != yend; ++y)
    {
        size_t xend = r.cols().end();
        for (size_t x = r.cols().begin(); x != xend; ++x)
        {
            size_t index = x + y*width;
            if (pred(m[index]))
            {
                if (Tag::is_final_scan())
                {
                    size_t idx = j*4;
                    values[idx] = (double)x;
                    values[idx + 1] = (double)y;
                    values[idx + 2] = p[index];
                    values[idx + 3] = 1.0;
                }
                ++j;
            }
        }
    }
    sum = j;
}

Я сейчас в 4 раза быстрее серийной версии. Что вы думаете об этом подходе? Я что-то пропустил и есть ли улучшения? Спасибо