Я использую открытый исходный код под названием ORB SLAM 2. Насколько мне известно, ORB SLAM 2 не сохраняет карту. Итак, чтобы сохранить точки (облака точек), я добавил небольшой код в System.cc:
void System::CreatePCD(const string &filename){
cout << endl << "Saving map points to " << filename << endl;
vector<MapPoint*> vMPs = mpMap->GetAllMapPoints();
// Create PCD init string
std::string begin = std::string("# .PCD v.7 - Point Cloud Data file format\nVERSON .7\n");
begin += "FIELDS x y z\n";
begin += "SIZE 4 4 4\n";
begin += "TYPE F F F\n";
begin += "COUNT 1 1 1\n";
int width = vMPs.size();
begin += "WIDTH ";
begin += std::to_string(width);
begin += "\nHEIGHT 1\n";
begin += "VIEWPOINT 0 0 0 1 0 0 0\n";
begin += "POINTS ";
begin += std::to_string(width);
begin += "\nDATA ascii\n";
// File Opening:
ofstream f;
f.open(filename.c_str());
f << begin;
// Write the point clouds:
for(size_t i= 0; i < vMPs.size(); ++i){
MapPoint *pMP = vMPs[i];
if (pMP->isBad()) continue;
cv::Mat MPPositions = pMP->GetWorldPos();
f << setprecision(7) << MPPositions.at<float>(0) << " " <<
MPPositions.at<float>(1) << " " << MPPositions.at<float>(2) << endl;
}
f.close();
cout << endl << "Map Points saved!" << endl;
}
}
Как видите, я добавил все необходимое для PCL версии 7. Мой недавно созданный файл облака точек выглядит так:
# .PCD v.7 - Point Cloud Data file format
VERSON .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 1287
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 1287
DATA ascii
0.1549043 -0.3846602 0.8497394
0.01127081 -0.2949406 0.9007485
0.6072361 -0.3651089 1.833479
…
Но всякий раз, когда я пытаюсь визуализировать файл, запустив pcl_viewer pointclouds.pcd
, я получаю сообщение об ошибке:
> Loading pointcloud.pcd [pcl::PCDReader::readHeader] No points to read
Что я делаю неправильно?
Похоже, у вас есть опечатка в вашем формате:
// Create PCD init string
std::string begin = std::string("# .PCD v.7 - Point Cloud Data file format\nVERSON .7\n");
VERSON
пишется вместо VERSION
(отсутствует I
).
Формат файла PCD (данных облака точек) - хорошо задокументированы. На первый взгляд ваш файл кажется правильным, но в вашем коде есть небольшая опечатка (отсутствует я):
std::string begin = std::string("# .PCD v.7 - Point Cloud Data file format\nVERSON .7\n");
Код пишет:
VERSON .7
Правильно было бы:
VERSION .7
Быстрый просмотр исходный код показывает, что опечатка приводит к раннему выходу, поскольку она не соответствует ни одному допустимому параметру. Это означает, что все следующие параметры, включая ТОЧКИ, игнорируются. Результатом будет ошибка Нет очков для чтения.
Исправьте опечатку, и ваш файл будет работать как положено.