mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-08 02:57:46 +08:00
Coloring scan (camera projection on point cloud) (#693)
* ExportClouds: Added camera projection options * ExportClouds: fixed ceiling/floor filtering options not saved in config * ExportClouds: fixed colorless scan points still exported when option is unchecked. * Export tool: added --bin, --poses, --images, --las and --cam_projection options; export with intensity with --scan option. PDALWriter: added binary option (only used for PLY an PCD formats). Rtabmap: Do graph optimization if neighbor link refined and Mem/UseOdomGravity is used.
This commit is contained in:
matlabbe
+179
-44
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <pdal/io/BufferReader.hpp>
|
||||
#include <pdal/StageFactory.hpp>
|
||||
#include <pdal/PluginManager.hpp>
|
||||
@@ -72,14 +73,31 @@ std::string getPDALSupportedWriters()
|
||||
return output;
|
||||
}
|
||||
|
||||
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud)
|
||||
int savePDALFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZ> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
bool binary)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
pdal::PointTable table;
|
||||
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z});
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::PointSourceId});
|
||||
}
|
||||
else
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z});
|
||||
}
|
||||
pdal::BufferReader bufferReader;
|
||||
|
||||
pdal::PointViewPtr view(new pdal::PointView(table));
|
||||
@@ -88,15 +106,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
|
||||
view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
|
||||
view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
|
||||
}
|
||||
}
|
||||
bufferReader.addView(view);
|
||||
|
||||
pdal::StageFactory factory;
|
||||
pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
|
||||
std::string ext = UFile::getExtension(filePath);
|
||||
pdal::Stage *writer = factory.createStage("writers." + ext);
|
||||
if(writer)
|
||||
{
|
||||
pdal::Options writerOps;
|
||||
writerOps.add("filename", filePath);
|
||||
if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
|
||||
if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
|
||||
|
||||
writer->setOptions(writerOps);
|
||||
writer->setInput(bufferReader);
|
||||
@@ -115,17 +140,37 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud)
|
||||
int savePDALFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
bool binary)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
pdal::PointTable table;
|
||||
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue});
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue,
|
||||
pdal::Dimension::Id::PointSourceId});
|
||||
}
|
||||
else
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue});
|
||||
}
|
||||
pdal::BufferReader bufferReader;
|
||||
|
||||
pdal::PointViewPtr view(new pdal::PointView(table));
|
||||
@@ -137,15 +182,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
|
||||
view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
|
||||
view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
|
||||
}
|
||||
}
|
||||
bufferReader.addView(view);
|
||||
|
||||
pdal::StageFactory factory;
|
||||
pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
|
||||
std::string ext = UFile::getExtension(filePath);
|
||||
pdal::Stage *writer = factory.createStage("writers." + ext);
|
||||
if(writer)
|
||||
{
|
||||
pdal::Options writerOps;
|
||||
writerOps.add("filename", filePath);
|
||||
if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
|
||||
if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
|
||||
|
||||
writer->setOptions(writerOps);
|
||||
writer->setInput(bufferReader);
|
||||
@@ -164,20 +216,43 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
|
||||
int savePDALFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
bool binary)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
pdal::PointTable table;
|
||||
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ});
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ,
|
||||
pdal::Dimension::Id::PointSourceId});
|
||||
}
|
||||
else
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Red,
|
||||
pdal::Dimension::Id::Green,
|
||||
pdal::Dimension::Id::Blue,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ});
|
||||
}
|
||||
pdal::BufferReader bufferReader;
|
||||
|
||||
pdal::PointViewPtr view(new pdal::PointView(table));
|
||||
@@ -192,15 +267,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
|
||||
view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
|
||||
view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
|
||||
}
|
||||
}
|
||||
bufferReader.addView(view);
|
||||
|
||||
pdal::StageFactory factory;
|
||||
pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
|
||||
std::string ext = UFile::getExtension(filePath);
|
||||
pdal::Stage *writer = factory.createStage("writers." + ext);
|
||||
if(writer)
|
||||
{
|
||||
pdal::Options writerOps;
|
||||
writerOps.add("filename", filePath);
|
||||
if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
|
||||
if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
|
||||
|
||||
writer->setOptions(writerOps);
|
||||
writer->setInput(bufferReader);
|
||||
@@ -219,15 +301,33 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud)
|
||||
int savePDALFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZI> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
bool binary)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
pdal::PointTable table;
|
||||
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity});
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity,
|
||||
pdal::Dimension::Id::PointSourceId});
|
||||
}
|
||||
else
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity});
|
||||
}
|
||||
pdal::BufferReader bufferReader;
|
||||
|
||||
pdal::PointViewPtr view(new pdal::PointView(table));
|
||||
@@ -237,15 +337,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
|
||||
view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
|
||||
view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
|
||||
}
|
||||
}
|
||||
bufferReader.addView(view);
|
||||
|
||||
pdal::StageFactory factory;
|
||||
pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
|
||||
std::string ext = UFile::getExtension(filePath);
|
||||
pdal::Stage *writer = factory.createStage("writers." + ext);
|
||||
if(writer)
|
||||
{
|
||||
pdal::Options writerOps;
|
||||
writerOps.add("filename", filePath);
|
||||
if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
|
||||
if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
|
||||
|
||||
writer->setOptions(writerOps);
|
||||
writer->setInput(bufferReader);
|
||||
@@ -264,18 +371,39 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
return 0; //success
|
||||
}
|
||||
|
||||
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud)
|
||||
int savePDALFile(const std::string & filePath,
|
||||
const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
|
||||
const std::vector<int> & cameraIds,
|
||||
bool binary)
|
||||
{
|
||||
UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
|
||||
uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
|
||||
|
||||
pdal::PointTable table;
|
||||
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ});
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ,
|
||||
pdal::Dimension::Id::PointSourceId});
|
||||
}
|
||||
else
|
||||
{
|
||||
table.layout()->registerDims({
|
||||
pdal::Dimension::Id::X,
|
||||
pdal::Dimension::Id::Y,
|
||||
pdal::Dimension::Id::Z,
|
||||
pdal::Dimension::Id::Intensity,
|
||||
pdal::Dimension::Id::NormalX,
|
||||
pdal::Dimension::Id::NormalY,
|
||||
pdal::Dimension::Id::NormalZ});
|
||||
}
|
||||
pdal::BufferReader bufferReader;
|
||||
|
||||
pdal::PointViewPtr view(new pdal::PointView(table));
|
||||
@@ -288,15 +416,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
|
||||
view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
|
||||
view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
|
||||
view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
|
||||
if(!cameraIds.empty())
|
||||
{
|
||||
view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
|
||||
}
|
||||
}
|
||||
bufferReader.addView(view);
|
||||
|
||||
pdal::StageFactory factory;
|
||||
pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
|
||||
std::string ext = UFile::getExtension(filePath);
|
||||
pdal::Stage *writer = factory.createStage("writers." + ext);
|
||||
if(writer)
|
||||
{
|
||||
pdal::Options writerOps;
|
||||
writerOps.add("filename", filePath);
|
||||
if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
|
||||
if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
|
||||
|
||||
writer->setOptions(writerOps);
|
||||
writer->setInput(bufferReader);
|
||||
|
||||
Reference in New Issue
Block a user