Преобразование облака точек в 2D-диапазон

Я пытаюсь преобразовать данные облака точек (x, y, z), полученные с Kinect V2, используя libfreenect2, в виртуальный 2D лазерное сканирование (например, вектор горизонтального угла / расстояния).

В настоящее время я назначаю для столбца пикселей значение расстояния PCL, как показано ниже:

std::vector<float> scan(512, 0);
for (unsigned int row = 0; row < 424; ++row) {
for (unsigned int col = 0; col < 512; ++col) {
float x, y, z;
registration->getPointXYZ(depth, row, col, x, y, z);
if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
continue;
}
Eigen::Vector3f values = rotate_translate((-1 * x), y - 1.186, z);
if (scan[col] == 0) {
scan[col] = values[1];
}
if (values[1] < scan[col]) {
scan[col] = values[1];
}
}
}

Вы можете игнорировать rotate_translate метод, он просто меняет локальные на глобальные координаты, используя позу датчика.

Проблема лучше всего показана на рисунках ниже:

Принимая во внимание, что датчик дальности LIDAR производит следующую карту точек:

2D-сканирование диапазона Kinect изогнутый, и, конечно, уже, поскольку горизонтальное поле обзора составляет 70,6 градуса по сравнению с диапазоном 270 градусов лидера.

Именно эту кривизну я пытаюсь исправить; библиотека SLAM / ICP, которую я использую, MRPT и фактические данные scan вставляется в mrpt::obs::CObservation2DRangeScan наблюдение:

auto obs = mrpt::obs::CObservation2DRangeScan();
obs.loadFromVectors(scan.size(), scan.data(), (char*)scan.data());
obs.aperture = mrpt::utils::DEG2RAD(70.6f);
obs.maxRange = 6.0;
obs.rightToLeft = true;
obs.timestamp = mrpt::system::now();
obs.setSensorPose(sensor);

Я искал вокруг Google и SO, и единственные ответы, которые, кажется, касаются этого вопроса, этот а также вон тот. Таким образом, хотя я понимаю, что кривизна является результатом того, что я назначаю каждому столбцу пикселей значение PCL, я не уверен, как можно использовать это для удаления кривизны.

Кажется, что каждый ответ использует другой подход, и, насколько я понимаю, задача состоит в линейная интерполяция угла на пиксель и текущих пиксельных координат?