i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-13 23:10:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Coloring scan (camera projection on point cloud) (#693)

Browse Source 
* 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
2021-03-01 09:56:37 -05:00
committed by GitHub
parent 967c57d165
commit d119487dd7
9 changed files with 1334 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/PDALWriter.h
+5 -5
View File
@@ -35,11 +35,11 @@ namespace rtabmap {
std::string getPDALSupportedWriters();
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
}
corelib/include/rtabmap/core/util3d.h
+28
View File
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
#include <opencv2/core/core.hpp>
#include <rtabmap/core/ProgressState.h>
#include <map>
#include <list>
@@ -300,6 +301,33 @@ void RTABMAP_EXP fillProjectedCloudHoles(
bool verticalDirection,
bool fillToBorder);
/**
* For each point, return pixel of the best camera (NodeID->CameraIndex)
* looking at it based on the policy and parameters
*/
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::map<int, Transform> & cameraPoses,
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance = 0.0f,
float maxAngle = 0.0f,
const std::vector<float> & roiRatios = std::vector<float>(),
bool distanceToCamPolicy = false,
const ProgressState * state = 0);
/**
* For each point, return pixel of the best camera (NodeID->CameraIndex)
* looking at it based on the policy and parameters
*/
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
const std::map<int, Transform> & cameraPoses,
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance = 0.0f,
float maxAngle = 0.0f,
const std::vector<float> & roiRatios = std::vector<float>(),
bool distanceToCamPolicy = false,
const ProgressState * state = 0);
bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
corelib/src/PDALWriter.cpp
+179 -44
View File
@@ -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);
corelib/src/Rtabmap.cpp
+4 -2
View File
@@ -1235,6 +1235,7 @@ bool Rtabmap::process(
bool smallDisplacement = false;
bool tooFastMovement = false;
std::list<int> signaturesRemoved;
bool neighborLinkRefined = false;
if(_rgbdSlamMode)
{
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1363,7 +1364,8 @@ bool Rtabmap::process(
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
neighborLinkRefined = !t.isNull();
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
@@ -2686,7 +2688,7 @@ bool Rtabmap::process(
lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
(signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
signaturesRetrieved.size()) // can be different map of the current one
corelib/src/util3d.cpp
+270 -46
View File
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
@@ -3015,52 +3016,6 @@ cv::Mat projectCloudToCamera(
{
UERROR("field map pcl::pointXYZ not found!");
}
/*
int count = 0;
for(int i=0; i<(int)laserScan->size(); ++i)
{
// Get 3D from laser scan
pcl::PointXYZ ptScan = laserScan->at(i);
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_low, dx_low);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_high, dx_high);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
if(set)
{
count++;
}
}
*/
UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
return registered;
@@ -3149,6 +3104,275 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
}
}
struct ProjectionInfo {
int nodeID;
int cameraIndex;
pcl::PointXY uv;
float distance;
};
/**
* For each point, return pixel of the best camera (NodeID->CameraIndex)
* looking at it based on the policy and parameters
*/
template<class PointT>
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamerasImpl (
const typename pcl::PointCloud<PointT> & cloud,
const std::map<int, Transform> & cameraPoses,
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
const std::vector<float> & roiRatios,
bool distanceToCamPolicy,
const ProgressState * state)
{
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
return pointToPixel;
std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
UINFO(msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
UWARN("Projecting to cameras cancelled!");
return pointToPixel;
}
std::vector<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras
int cameraProcessed = 0;
for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
{
std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin();
if(iter!=cameraModels.end() && !iter->second.empty())
{
for(size_t i=0; i<iter->second.size(); ++i)
{
Transform cameraTransform = (pter->second * iter->second[i].localTransform());
UASSERT(!cameraTransform.isNull());
cv::Mat cameraMatrixK = iter->second[i].K();
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
const cv::Size & imageSize = iter->second[i].imageSize();
float fx = cameraMatrixK.at<double>(0,0);
float fy = cameraMatrixK.at<double>(1,1);
float cx = cameraMatrixK.at<double>(0,2);
float cy = cameraMatrixK.at<double>(1,2);
// depth: 2 channels UINT: [depthMM, indexPt]
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
Transform t = cameraTransform.inverse();
cv::Rect roi(0,0,imageSize.width, imageSize.height);
if(roiRatios.size()==4)
{
roi = util2d::computeRoi(imageSize, roiRatios);
}
int count = 0;
for(size_t i=0; i<cloud.size(); ++i)
{
// Get 3D from laser scan
PointT ptScan = cloud.at(i);
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
int zMM = z * 1000;
if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height))
{
set = true;
cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
if(zReg[0] == 0 || zMM < zReg[0])
{
zReg[0] = zMM;
zReg[1] = i;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height))
{
set = true;
cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
if(zReg[0] == 0 || zMM < zReg[0])
{
zReg[0] = zMM;
zReg[1] = i;
}
}
}
if(set)
{
count++;
}
}
if(count == 0)
{
registered = cv::Mat();
UINFO("No points projected in camera %d/%d", pter->first, i);
}
else
{
UDEBUG("%d points projected in camera %d/%d", count, pter->first, i);
}
for(int u=0; u<registered.cols; ++u)
{
for(int v=0; v<registered.rows; ++v)
{
cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
if(zReg[0] > 0)
{
ProjectionInfo info;
info.nodeID = pter->first;
info.cameraIndex = i;
info.uv.x = float(u)/float(imageSize.width);
info.uv.y = float(v)/float(imageSize.height);
info.distance = zReg[0]/1000.0f;
invertedIndex[zReg[1]].push_back(info);
}
}
}
}
}
msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
UINFO(msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
UWARN("Projecting to cameras cancelled!");
return pointToPixel;
}
++cameraProcessed;
}
msg = uFormat("Select best camera for %d points...", (int)cloud.size());
UINFO(msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
UWARN("Projecting to cameras cancelled!");
return pointToPixel;
}
pointToPixel.resize(invertedIndex.size());
int colorized = 0;
// For each point
for(size_t i=0; i<invertedIndex.size(); ++i)
{
if((i+1)%10000 == 0)
{
UDEBUG("Point %d/%d", i+1, (int)cloud.size());
if(state && !state->callback(uFormat("%d/%d points projected to cameras (out of %d points)", colorized, i+1, (int)cloud.size())))
{
//cancelled!
UWARN("Projecting to camera cancelled!");
pointToPixel.clear();
return pointToPixel;
}
}
const PointT & pt = cloud.at(i);
int nodeID = -1;
int cameraIndex = -1;
float smallestWeight = std::numeric_limits<float>::max();
pcl::PointXY uv_coords;
for (size_t j = 0; j<invertedIndex[i].size(); ++j)
{
const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID);
Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
float angleToCam = pcl::getAngle3D(normal, camDir);
float distanceToCam = invertedIndex[i][j].distance;
if(camDir.dot(normal) > 0 && // is facing camera?
(maxAngle<=0 || angleToCam < maxAngle) && // is point normal perpendicular to camera?
(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
{
float vx = invertedIndex[i][j].uv.x-0.5f;
float vy = invertedIndex[i][j].uv.y-0.5f;
float distanceToCenter = vx*vx+vy*vy;
float distance = distanceToCenter;
if(distanceToCamPolicy)
{
distance = distanceToCam;
}
if(distance <= smallestWeight)
{
nodeID = invertedIndex[i][j].nodeID;
cameraIndex = invertedIndex[i][j].cameraIndex;
smallestWeight = distance;
uv_coords = invertedIndex[i][j].uv;
}
}
}
if(nodeID>-1 && cameraIndex> -1)
{
pointToPixel[i].first.first = nodeID;
pointToPixel[i].first.second = cameraIndex;
pointToPixel[i].second = uv_coords;
++colorized;
}
}
UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
return pointToPixel;
}
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::map<int, Transform> & cameraPoses,
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
const std::vector<float> & roiRatios,
bool distanceToCamPolicy,
const ProgressState * state)
{
return projectCloudToCamerasImpl(cloud,
cameraPoses,
cameraModels,
maxDistance,
maxAngle,
roiRatios,
distanceToCamPolicy,
state);
}
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud,
const std::map<int, Transform> & cameraPoses,
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
const std::vector<float> & roiRatios,
bool distanceToCamPolicy,
const ProgressState * state)
{
return projectCloudToCamerasImpl(cloud,
cameraPoses,
cameraModels,
maxDistance,
maxAngle,
roiRatios,
distanceToCamPolicy,
state);
}
bool isFinite(const cv::Point3f & pt)
{
return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
guilib/include/rtabmap/gui/ExportCloudsDialog.h
+1 -1
View File
@@ -138,7 +138,7 @@ private:
const std::map<int, LaserScan> & cachedScans,
const ParametersMap & parameters,
bool & has2dScans) const;
void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true, const std::vector<std::map<int, pcl::PointXY> > & pointToPixels = std::vector<std::map<int, pcl::PointXY> >());
void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
guilib/src/ExportCloudsDialog.cpp
+307 -22
View File
@@ -115,6 +115,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_ceilingHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_floorHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->spinBox_decimation_scan, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_rangeMin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_rangeMax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -173,9 +175,23 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->checkBox_blending, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_blendingDecimation, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->lineEdit_camProjRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_camProjExportCamera, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
#ifndef RTABMAP_PDAL
_ui->comboBox_camProjExportCamera->setEnabled(false);
_ui->label_camProjExportCamera->setEnabled(false);
_ui->label_camProjExportCamera->setText(_ui->label_camProjExportCamera->text() + " (PDAL dependency required)");
#endif
connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_meshDecimationFactor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -313,6 +329,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value());
settings.setValue("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value());
settings.setValue("regenerate_floor", _ui->doubleSpinBox_floorHeight->value());
settings.setValue("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value());
settings.setValue("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value());
settings.setValue("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value());
@@ -356,6 +374,15 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("gain_rgb", _ui->checkBox_gainRGB->isChecked());
settings.setValue("gain_full", _ui->checkBox_gainFull->isChecked());
settings.setValue("cam_proj", _ui->checkBox_cameraProjection->isChecked());
settings.setValue("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text());
settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked());
settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked());
settings.setValue("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex());
settings.setValue("mesh", _ui->checkBox_meshing->isChecked());
settings.setValue("mesh_radius", _ui->doubleSpinBox_gp3Radius->value());
settings.setValue("mesh_mu", _ui->doubleSpinBox_gp3Mu->value());
@@ -451,6 +478,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
_ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
_ui->doubleSpinBox_ceilingHeight->setValue(settings.value("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()).toDouble());
_ui->doubleSpinBox_floorHeight->setValue(settings.value("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()).toDouble());
_ui->spinBox_decimation_scan->setValue(settings.value("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()).toInt());
_ui->doubleSpinBox_rangeMax->setValue(settings.value("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()).toDouble());
_ui->doubleSpinBox_rangeMin->setValue(settings.value("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()).toDouble());
@@ -497,6 +526,15 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_ui->checkBox_gainRGB->setChecked(settings.value("gain_rgb", _ui->checkBox_gainRGB->isChecked()).toBool());
_ui->checkBox_gainFull->setChecked(settings.value("gain_full", _ui->checkBox_gainFull->isChecked()).toBool());
_ui->checkBox_cameraProjection->setChecked(settings.value("cam_proj", _ui->checkBox_cameraProjection->isChecked()).toBool());
_ui->lineEdit_camProjRoiRatios->setText(settings.value("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text()).toString());
_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool());
_ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool());
_ui->comboBox_camProjExportCamera->setCurrentIndex(settings.value("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex()).toInt());
_ui->checkBox_meshing->setChecked(settings.value("mesh", _ui->checkBox_meshing->isChecked()).toBool());
_ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble());
_ui->doubleSpinBox_gp3Mu->setValue(settings.value("mesh_mu", _ui->doubleSpinBox_gp3Mu->value()).toDouble());
@@ -592,6 +630,8 @@ void ExportCloudsDialog::restoreDefaults()
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4);
_ui->doubleSpinBox_minDepth->setValue(0);
_ui->doubleSpinBox_ceilingHeight->setValue(0);
_ui->doubleSpinBox_floorHeight->setValue(0);
_ui->spinBox_decimation_scan->setValue(1);
_ui->doubleSpinBox_rangeMax->setValue(0);
_ui->doubleSpinBox_rangeMin->setValue(0);
@@ -635,6 +675,15 @@ void ExportCloudsDialog::restoreDefaults()
_ui->checkBox_gainRGB->setChecked(true);
_ui->checkBox_gainFull->setChecked(false);
_ui->checkBox_cameraProjection->setChecked(false);
_ui->lineEdit_camProjRoiRatios->setText("0.0 0.0 0.0 0.0");
_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false);
_ui->checkBox_camProjRecolorPoints->setChecked(true);
_ui->comboBox_camProjExportCamera->setCurrentIndex(0);
_ui->checkBox_meshing->setChecked(false);
_ui->doubleSpinBox_gp3Radius->setValue(0.2);
_ui->doubleSpinBox_gp3Mu->setValue(2.5);
@@ -778,11 +827,15 @@ void ExportCloudsDialog::updateReconstructionFlavor()
_ui->checkBox_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled());
_ui->label_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled());
_ui->checkBox_cameraProjection->setEnabled(_ui->checkBox_assemble->isChecked() && !_ui->checkBox_meshing->isChecked());
_ui->label_cameraProjection->setEnabled(_ui->checkBox_cameraProjection->isEnabled());
_ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked() && _ui->checkBox_fromDepth->isChecked());
_ui->groupBox_regenerateScans->setVisible(_ui->checkBox_regenerate->isChecked() && !_ui->checkBox_fromDepth->isChecked());
_ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked());
_ui->groupBox_filtering->setVisible(_ui->checkBox_filtering->isChecked());
_ui->groupBox_gain->setVisible(_ui->checkBox_gainCompensation->isEnabled() && _ui->checkBox_gainCompensation->isChecked());
_ui->groupBox_cameraProjection->setVisible(_ui->checkBox_cameraProjection->isEnabled() && _ui->checkBox_cameraProjection->isChecked());
_ui->groupBox_mls->setVisible(_ui->checkBox_smoothing->isEnabled() && _ui->checkBox_smoothing->isChecked());
_ui->groupBox_meshing->setVisible(_ui->checkBox_meshing->isChecked());
_ui->groupBox_subtraction->setVisible(_ui->checkBox_subtraction->isChecked());
@@ -952,7 +1005,7 @@ void ExportCloudsDialog::exportClouds(
}
else
{
saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked());
saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked(), textureVertexToPixels);
}
}
else if(!_canceled)
@@ -1234,16 +1287,13 @@ void ExportCloudsDialog::viewClouds(
_progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
_progressDialog->incrementStep();
QColor color = Qt::gray;
int mapId = uValue(mapIds, iter->first, -1);
if(mapId >= 0)
if(!_ui->checkBox_fromDepth->isChecked() &&
!(_ui->checkBox_cameraProjection->isEnabled() &&
_ui->checkBox_cameraProjection->isChecked() &&
_ui->checkBox_camProjRecolorPoints->isChecked() &&
clouds.size()==1 && clouds.begin()->first==0))
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
if(!_ui->checkBox_fromDepth->isChecked())
{
// When laser scans are exported, convert RGB to Intensity
// When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity
if(_ui->spinBox_normalKSearch->value()<=0 && _ui->doubleSpinBox_normalRadiusSearch->value()<=0.0)
{
// remove normals if not used
@@ -1709,6 +1759,11 @@ bool ExportCloudsDialog::getExportedClouds(
if(!_ui->checkBox_fromDepth->isChecked() && !has2dScans &&
(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0))
{
_progressDialog->appendText(tr("Computing normals (%1 points, K=%2, radius=%3 m)...")
.arg(assembledCloud->size())
.arg(_ui->spinBox_normalKSearch->value())
.arg(_ui->doubleSpinBox_normalRadiusSearch->value()));
// recompute normals
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
@@ -2444,6 +2499,217 @@ bool ExportCloudsDialog::getExportedClouds(
_progressDialog->appendText(msg.c_str(), Qt::darkYellow);
UWARN(msg.c_str());
}
else if(_ui->checkBox_cameraProjection->isEnabled() &&
_ui->checkBox_cameraProjection->isChecked() &&
cloudsWithNormals.size()==1 && cloudsWithNormals.begin()->first==0) // only for assembled point cloud
{
_progressDialog->appendText(tr("Camera projection..."));
QApplication::processEvents();
uSleep(100);
QApplication::processEvents();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloud = cloudsWithNormals.begin()->second;
std::map<int, Transform> cameraPoses;
std::map<int, std::vector<CameraModel> > cameraModels;
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(stereoModel.isValidForProjection())
{
models.clear();
models.push_back(stereoModel.left());
}
else if(models.size() == 0 || !models[0].isValidForProjection())
{
models.clear();
}
if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
{
cameraPoses.insert(std::make_pair(iter->first, iter->second));
cameraModels.insert(std::make_pair(iter->first, models));
}
else
{
UWARN("%d node has invalid camera models, ignoring it.", iter->first);
}
}
if(!cameraPoses.empty())
{
TexturingState texturingState(_progressDialog, true);
_progressDialog->setMaximumSteps(_progressDialog->maximumSteps() + assembledCloud->size()/10000+1 + cameraPoses.size());
std::vector<float> roiRatios;
QStringList strings = _ui->lineEdit_camProjRoiRatios->text().split(' ');
if(!_ui->lineEdit_meshingTextureRoiRatios->text().isEmpty())
{
if(strings.size()==4)
{
roiRatios.resize(4);
roiRatios[0]=strings[0].toDouble();
roiRatios[1]=strings[1].toDouble();
roiRatios[2]=strings[2].toDouble();
roiRatios[3]=strings[3].toDouble();
if(!(roiRatios[0]>=0.0f && roiRatios[0]<=1.0f &&
roiRatios[1]>=0.0f && roiRatios[1]<=1.0f &&
roiRatios[2]>=0.0f && roiRatios[2]<=1.0f &&
roiRatios[3]>=0.0f && roiRatios[3]<=1.0f))
{
roiRatios.clear();
}
}
if(roiRatios.empty())
{
QString msg = tr("Wrong ROI format. Region of Interest (ROI) must "
"have 4 values [left right top bottom] between 0 and 1 "
"separated by space (%1), ignoring it for projecting cameras...")
.arg(_ui->lineEdit_camProjRoiRatios->text());
UWARN(msg.toStdString().c_str());
_progressDialog->appendText(msg, Qt::darkYellow);
_progressDialog->setAutoClose(false);
}
}
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
pointToPixel = util3d::projectCloudToCameras(
*assembledCloud,
cameraPoses,
cameraModels,
_ui->doubleSpinBox_camProjMaxDistance->value(),
_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
roiRatios,
_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),
&texturingState);
// color the cloud
UASSERT(pointToPixel.empty() || pointToPixel.size() == assembledCloud->size());
QMap<int, cv::Mat> cachedImages;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints;
if(!_ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked())
{
assembledCloudValidPoints.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
assembledCloudValidPoints->resize(assembledCloud->size());
}
if(_ui->comboBox_camProjExportCamera->isEnabled() &&
_ui->comboBox_camProjExportCamera->currentIndex()>0)
{
textureVertexToPixels.resize(assembledCloud->size());
}
int oi=0;
for(size_t i=0; i<pointToPixel.size(); ++i)
{
pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked())
{
pt.r = 255;
pt.g = 0;
pt.b = 0;
pt.a = 255;
}
int nodeID = pointToPixel[i].first.first;
int cameraIndex = pointToPixel[i].first.second;
if(nodeID>0 && cameraIndex>=0)
{
if(_ui->checkBox_camProjRecolorPoints->isChecked())
{
cv::Mat image;
if(cachedImages.contains(nodeID))
{
image = cachedImages.value(nodeID);
}
else if(cachedSignatures.contains(nodeID) && !cachedSignatures.value(nodeID).sensorData().imageCompressed().empty())
{
cachedSignatures.value(nodeID).sensorData().uncompressDataConst(&image, 0);
cachedImages.insert(nodeID, image);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(nodeID, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
cachedImages.insert(nodeID, image);
}
if(!image.empty())
{
int subImageWidth = image.cols / cameraModels.at(nodeID).size();
image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
int x = pointToPixel[i].second.x * (float)image.cols;
int y = pointToPixel[i].second.y * (float)image.rows;
UASSERT(x>=0 && x<image.cols);
UASSERT(y>=0 && y<image.rows);
if(image.type()==CV_8UC3)
{
cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
pt.b = bgr[0];
pt.g = bgr[1];
pt.r = bgr[2];
}
else
{
UASSERT(image.type()==CV_8UC1);
pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
}
}
}
int exportedId = nodeID;
if(_ui->comboBox_camProjExportCamera->currentIndex() == 2)
{
exportedId = cameraIndex+1;
}
else if(_ui->comboBox_camProjExportCamera->currentIndex() == 3)
{
UASSERT_MSG(cameraIndex<10, "Exporting cam id as NodeId+CamIndex is only supported when the number of cameras per node < 10.");
exportedId = nodeID*10 + cameraIndex;
}
if(assembledCloudValidPoints.get())
{
if(!textureVertexToPixels.empty())
{
textureVertexToPixels[oi].insert(std::make_pair(exportedId, pointToPixel[i].second));
}
assembledCloudValidPoints->at(oi++) = pt;
}
else if(!textureVertexToPixels.empty())
{
textureVertexToPixels[i].insert(std::make_pair(exportedId, pointToPixel[i].second));
}
}
}
if(assembledCloudValidPoints.get())
{
assembledCloudValidPoints->resize(oi);
cloudsWithNormals.begin()->second = assembledCloudValidPoints;
if(!textureVertexToPixels.empty())
{
textureVertexToPixels.resize(oi);
}
}
if(_canceled)
{
return false;
}
}
}
UDEBUG("");
#ifdef RTABMAP_CPUTSDF
@@ -2933,7 +3199,10 @@ bool ExportCloudsDialog::getExportedClouds(
}
if(roiRatios.empty())
{
QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 values [left right top bottom] between 0 and 1 separated by space (%1), ignoring it for texturing...").arg(_ui->lineEdit_meshingTextureRoiRatios->text());
QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 "
"values [left right top bottom] between 0 and 1 "
"separated by space (%1), ignoring it for texturing...")
.arg(_ui->lineEdit_meshingTextureRoiRatios->text());
UWARN(msg.toStdString().c_str());
_progressDialog->appendText(msg, Qt::darkYellow);
_progressDialog->setAutoClose(false);
@@ -3015,8 +3284,8 @@ bool ExportCloudsDialog::getExportedClouds(
textureMeshes.clear();
textureMeshes.insert(std::make_pair(0, assembledMesh));
}
}
UDEBUG("");
return true;
}
@@ -3401,7 +3670,8 @@ void ExportCloudsDialog::saveClouds(
const QString & workingDirectory,
const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
bool binaryMode)
bool binaryMode,
const std::vector<std::map<int, pcl::PointXY> > & pointToPixels)
{
if(clouds.size() == 1)
{
@@ -3435,9 +3705,13 @@ void ExportCloudsDialog::saveClouds(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
if(!_ui->checkBox_fromDepth->isChecked())
if(!_ui->checkBox_fromDepth->isChecked() &&
!(_ui->checkBox_cameraProjection->isEnabled() &&
_ui->checkBox_cameraProjection->isChecked() &&
_ui->checkBox_camProjRecolorPoints->isChecked() &&
clouds.size()==1 && clouds.begin()->first==0))
{
// When laser scans are exported, convert RGB to Intensity
// When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity
if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
{
cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
@@ -3505,8 +3779,11 @@ void ExportCloudsDialog::saveClouds(
success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
}
}
else if(QFileInfo(path).suffix() == "ply")
{
#ifdef RTABMAP_PDAL
else if(QFileInfo(path).suffix() == "ply" && pointToPixels.empty()) {
#else
else if(QFileInfo(path).suffix() == "ply") {
#endif
if(cloudIWithNormals.get())
{
success = pcl::io::savePLYFile(path.toStdString(), *cloudIWithNormals, binaryMode) == 0;
@@ -3527,21 +3804,29 @@ void ExportCloudsDialog::saveClouds(
#ifdef RTABMAP_PDAL
else if(!QFileInfo(path).suffix().isEmpty())
{
std::vector<int> cameraIds(pointToPixels.size(), 0);
for(size_t i=0;i<pointToPixels.size(); ++i)
{
if(!pointToPixels[i].empty())
{
cameraIds[i] = pointToPixels[i].begin()->first;
}
}
if(cloudIWithNormals.get())
{
success = savePDALFile(path.toStdString(), *cloudIWithNormals) == 0;
success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0;
}
else if(cloudIWithoutNormals.get())
{
success = savePDALFile(path.toStdString(), *cloudIWithoutNormals) == 0;
success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0;
}
else if(cloudRGBWithoutNormals.get())
{
success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals) == 0;
success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0;
}
else
{
success = savePDALFile(path.toStdString(), *clouds.begin()->second) == 0;
success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0;
}
}
#endif
guilib/src/ui/exportCloudsDialog.ui
+240 -38
View File
@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-584</y>
<width>780</width>
<height>4975</height>
<height>5347</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -76,13 +76,6 @@
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QCheckBox" name="checkBox_gainCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_voxel">
<property name="text">
@@ -103,16 +96,6 @@
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_gainCompensation">
<property name="text">
<string>Gain compensation. Normalize brightness of images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_frame">
<property name="text">
@@ -133,16 +116,6 @@
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_binaryFile_12">
<property name="text">
<string>Meshing.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="checkBox_smoothing">
<property name="text">
@@ -160,13 +133,6 @@
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QCheckBox" name="checkBox_meshing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_binaryFile">
<property name="text">
@@ -323,6 +289,57 @@
</item>
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_gainCompensation">
<property name="text">
<string>Gain compensation. Normalize brightness of images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_cameraProjection">
<property name="text">
<string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QCheckBox" name="checkBox_gainCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QCheckBox" name="checkBox_cameraProjection">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_binaryFile_12">
<property name="text">
<string>Meshing.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QCheckBox" name="checkBox_meshing">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -642,8 +659,11 @@
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
@@ -661,8 +681,11 @@
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
@@ -1475,6 +1498,185 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_cameraProjection">
<property name="title">
<string>Camera Projection</string>
</property>
<property name="checkable">
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
<property name="suffix">
<string> deg</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_15">
<property name="text">
<string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_14">
<property name="text">
<string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_13">
<property name="text">
<string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_12">
<property name="text">
<string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="2" colspan="2">
<widget class="QLabel" name="label_camProjExportCamera">
<property name="text">
<string>ID format of the camera selected for each point.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QComboBox" name="comboBox_camProjExportCamera">
<property name="toolTip">
<string>By Node ID: cameras of same node have same ID
By Camera Index: of a multi-cameras setup
By Node ID and Camera Index: NodeID*10+CameraIndex</string>
</property>
<item>
<property name="text">
<string>Disabled</string>
</property>
</item>
<item>
<property name="text">
<string>By Node ID</string>
</property>
</item>
<item>
<property name="text">
<string>By Camera Index</string>
</property>
</item>
<item>
<property name="text">
<string>By Node ID and Camera Index</string>
</property>
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
</item>
<item row="1" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_17">
<property name="text">
<string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLabel" name="label_meshingTextureSize_18">
<property name="text">
<string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_meshing">
<property name="title">
tools/Export/main.cpp
+300 -34
View File
@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/optimizer/OptimizerG2O.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
@@ -43,19 +45,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/poisson.h>
#include <stdio.h>
#ifdef RTABMAP_PDAL
#include <rtabmap/core/PDALWriter.h>
#endif
using namespace rtabmap;
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-exportCloud [options] database.db\n"
"rtabmap-export [options] database.db\n"
"Options:\n"
" --bin Export PLY in binary format.\n"
" --las Export cloud in LAS instead of PLY (PDAL dependency required).\n"
" --mesh Create a mesh.\n"
" --texture Create a mesh with texture.\n"
" --texture_size # Texture size (default 4096).\n"
" --texture_count # Maximum textures generated (default 1).\n"
" --texture_range # Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
" --texture_d2c Distance to camera policy.\n"
" --cam_projection Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
" --poses Export optimized poses in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n"
" --images Export images.\n"
" --ba Do global bundle adjustment before assembling the clouds.\n"
" --gain # Gain compensation value (default 1, set 0 to disable).\n"
" --gain_gray Do gain estimation compensation on gray channel only (default RGB channels).\n"
@@ -87,6 +98,8 @@ int main(int argc, char * argv[])
showUsage();
}
bool binary = false;
bool las = false;
bool mesh = false;
bool texture = false;
bool ba = false;
@@ -109,12 +122,28 @@ int main(int argc, char * argv[])
bool saveInDb = false;
int lowBrightnessGain = 0;
int highBrightnessGain = 10;
bool camProjection = false;
bool exportPoses = false;
bool exportImages = false;
for(int i=1; i<argc; ++i)
{
if(std::strcmp(argv[i], "--help") == 0)
{
showUsage();
}
else if(std::strcmp(argv[i], "--bin") == 0)
{
binary = true;
}
else if(std::strcmp(argv[i], "--las") == 0)
{
#ifdef RTABMAP_PDAL
las = true;
#else
printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
#endif
}
else if(std::strcmp(argv[i], "--mesh") == 0)
{
mesh = true;
@@ -164,6 +193,18 @@ int main(int argc, char * argv[])
{
distanceToCamPolicy = true;
}
else if(std::strcmp(argv[i], "--cam_projection") == 0)
{
camProjection = true;
}
else if(std::strcmp(argv[i], "--poses") == 0)
{
exportPoses = true;
}
else if(std::strcmp(argv[i], "--images") == 0)
{
exportImages = true;
}
else if(std::strcmp(argv[i], "--ba") == 0)
{
ba = true;
@@ -409,35 +450,45 @@ int main(int argc, char * argv[])
// Construct the cloud
printf("Create and assemble the clouds...\n");
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>);
std::map<int, rtabmap::Transform> cameraPoses;
std::map<int, double> cameraStamps;
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
std::map<int, cv::Mat> cameraDepths;
int imagesExported = 0;
for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
{
Signature node = nodes.find(iter->first)->second;
// uncompress data
std::vector<CameraModel> models = node.sensorData().cameraModels();
cv::Mat rgb;
cv::Mat depth;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
if(cloudFromScan)
{
cv::Mat tmpDepth;
LaserScan scan;
node.sensorData().uncompressData(0, texture&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
if(decimation>1 || maxRange)
{
scan = util3d::commonFiltering(scan, decimation, 0, maxRange);
}
cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
if(scan.hasRGB())
{
cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
}
else
{
cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
}
}
else
{
cv::Mat tmpRGB;
node.sensorData().uncompressData(&tmpRGB, &depth);
node.sensorData().uncompressData(&rgb, &depth);
cloud = util3d::cloudRGBFromSensorData(
node.sensorData(),
decimation, // image decimation before creating the clouds
@@ -445,11 +496,55 @@ int main(int argc, char * argv[])
0.0f,
indices.get());
}
if(exportImages && !rgb.empty())
{
std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1)?"rgb":"left";
std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix;
UDirectory::makeDir(dir);
std::string outputPath=dir+"/"+uNumber2Str(iter->first)+".jpg";
cv::imwrite(outputPath, rgb);
++imagesExported;
if(!depth.empty())
{
std::string ext;
cv::Mat depthExported = depth;
if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1)
{
ext = ".jpg";
dir = outputDirectory+"/"+baseName+"_right";
}
else
{
ext = ".png";
dir = outputDirectory+"/"+baseName+"_depth";
if(depth.type() == CV_32FC1)
{
depthExported = rtabmap::util2d::cvtDepthFromFloat(depth);
}
}
if(!UDirectory::exists(dir))
{
UDirectory::makeDir(dir);
}
outputPath=dir+"/"+uNumber2Str(iter->first)+ext;
cv::imwrite(outputPath, depthExported);
}
}
if(voxelSize>0.0f)
{
cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
if(cloud.get() && !cloud->empty())
cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
else if(cloudI.get() && !cloudI->empty())
cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
}
cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
if(cloud.get() && !cloud->empty())
cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
else if(cloudI.get() && !cloudI->empty())
cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
if(cloudFromScan)
@@ -457,21 +552,37 @@ int main(int argc, char * argv[])
Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
viewpoint = Eigen::Vector3f(iter->second.x(), iter->second.y(), iter->second.z());
}
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
if(mergedClouds->size() == 0)
if(cloud.get() && !cloud->empty())
{
*mergedClouds = *cloudWithNormals;
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
if(mergedClouds->size() == 0)
{
*mergedClouds = *cloudWithNormals;
}
else
{
*mergedClouds += *cloudWithNormals;
}
}
else
else if(cloudI.get() && !cloudI->empty())
{
*mergedClouds += *cloudWithNormals;
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
if(mergedCloudsI->size() == 0)
{
*mergedCloudsI = *cloudIWithNormals;
}
else
{
*mergedCloudsI += *cloudIWithNormals;
}
}
cameraPoses.insert(std::make_pair(iter->first, iter->second));
cameraStamps.insert(std::make_pair(iter->first, node.getStamp()));
if(!models.empty())
{
cameraModels.insert(std::make_pair(iter->first, models));
@@ -481,9 +592,12 @@ int main(int argc, char * argv[])
cameraDepths.insert(std::make_pair(iter->first, depth));
}
}
printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), (int)mergedClouds->size());
printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size());
if(mergedClouds->size())
if(imagesExported>0)
printf("%d images exported!\n", imagesExported);
if(!mergedClouds->empty() || !mergedCloudsI->empty())
{
if(saveInDb)
{
@@ -495,31 +609,180 @@ int main(int argc, char * argv[])
driver->save2DMap(cv::Mat(), 0, 0, 0);
driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
}
if(!(mesh || texture))
else if(exportPoses)
{
if(voxelSize>0.0f)
std::string outputPath=outputDirectory+"/"+baseName+"_poses.txt";
rtabmap::graph::exportPoses(outputPath, 10, cameraPoses, std::multimap<int, Link>(), cameraStamps);
printf("Poses exported to \"%s\".\n", outputPath.c_str());
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
if(voxelSize>0.0f)
{
printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
if(!cloudToExport->empty())
{
printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", voxelSize, (int)mergedClouds->size());
mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
cloudToExport = util3d::voxelize(cloudToExport, voxelSize);
}
else if(!cloudIToExport->empty())
{
cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize);
}
printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
}
std::vector<int> pointToCamId;
if(camProjection && !cameraPoses.empty())
{
printf("Camera projection...\n");
pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
if(!cloudToExport->empty())
{
pointToPixel = util3d::projectCloudToCameras(
*cloudToExport,
cameraPoses,
cameraModels,
textureRange,
0,
std::vector<float>(),
distanceToCamPolicy);
}
else if(!cloudIToExport->empty())
{
pointToPixel = util3d::projectCloudToCameras(
*cloudIToExport,
cameraPoses,
cameraModels,
textureRange,
0,
std::vector<float>(),
distanceToCamPolicy);
}
if(saveInDb)
// color the cloud
UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size());
std::map<int, cv::Mat> cachedImages;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
assembledCloudValidPoints->resize(pointToCamId.size());
int oi=0;
for(size_t i=0; i<pointToPixel.size(); ++i)
{
printf("Saving in db... (%d points)\n", (int)mergedClouds->size());
driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*mergedClouds, Transform(), false).data());
printf("Saving in db... done!\n");
pcl::PointXYZRGBNormal pt;
if(!cloudToExport->empty())
{
pt = cloudToExport->at(i);
}
else if(!cloudIToExport->empty())
{
pt.x = cloudIToExport->at(i).x;
pt.y = cloudIToExport->at(i).y;
pt.z = cloudIToExport->at(i).z;
pt.normal_x = cloudIToExport->at(i).normal_x;
pt.normal_y = cloudIToExport->at(i).normal_y;
pt.normal_z = cloudIToExport->at(i).normal_z;
}
int nodeID = pointToPixel[i].first.first;
int cameraIndex = pointToPixel[i].first.second;
if(nodeID>0 && cameraIndex>=0)
{
cv::Mat image;
if(uContains(cachedImages, nodeID))
{
image = cachedImages.at(nodeID);
}
else if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty())
{
nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0);
cachedImages.insert(std::make_pair(nodeID, image));
}
if(!image.empty())
{
int subImageWidth = image.cols / cameraModels.at(nodeID).size();
image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
int x = pointToPixel[i].second.x * (float)image.cols;
int y = pointToPixel[i].second.y * (float)image.rows;
UASSERT(x>=0 && x<image.cols);
UASSERT(y>=0 && y<image.rows);
if(image.type()==CV_8UC3)
{
cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
pt.b = bgr[0];
pt.g = bgr[1];
pt.r = bgr[2];
}
else
{
UASSERT(image.type()==CV_8UC1);
pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
}
}
int exportedId = nodeID;
pointToCamId[oi] = exportedId;
assembledCloudValidPoints->at(oi++) = pt;
}
}
else
assembledCloudValidPoints->resize(oi);
cloudToExport = assembledCloudValidPoints;
cloudIToExport->clear();
pointToCamId.resize(oi);
printf("Camera projection... done! (%fs)\n", timer.ticks());
}
if(saveInDb)
{
if(!(mesh || texture))
{
std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply";
printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size());
pcl::io::savePLYFile(outputPath, *mergedClouds);
printf("Saving %s... done!\n", outputPath.c_str());
printf("Saving in db... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
if(!cloudToExport->empty())
driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudToExport, Transform(), false).data());
else if(!cloudIToExport->empty())
driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudIToExport, Transform(), false).data());
printf("Saving in db... done!\n");
}
}
else
{
std::string ext = las?"las":"ply";
std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext;
printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
#ifdef RTABMAP_PDAL
if(!cloudToExport->empty())
savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
else if(!cloudIToExport->empty())
savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
#else
if(!pointToCamId.empty())
{
printf("Option --cam_projection is enabled but rtabmap is not built "
"with PDAL support, so camera IDs won't be exported in the output cloud.\n");
}
if(!cloudToExport->empty())
pcl::io::savePLYFile(outputPath, *cloudToExport, binary);
else if(!cloudIToExport->empty())
pcl::io::savePLYFile(outputPath, *cloudIToExport, binary);
#endif
printf("Saving %s... done!\n", outputPath.c_str());
}
// Meshing...
if(mesh || texture)
{
if(!mergedCloudsI->empty())
{
pcl::copyPointCloud(*mergedCloudsI, *mergedClouds);
mergedCloudsI->clear();
}
Eigen::Vector4f min,max;
pcl::getMinMax3D(*mergedClouds, min, max);
float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
@@ -574,7 +837,10 @@ int main(int argc, char * argv[])
{
std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
printf("Saving %s...\n", outputPath.c_str());
pcl::io::savePLYFile(outputPath, *mesh);
if(binary)
pcl::io::savePLYFileBinary(outputPath, *mesh);
else
pcl::io::savePLYFile(outputPath, *mesh);
printf("Saving %s... done!\n", outputPath.c_str());
}
}