i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

10
corelib/include/rtabmap/core/PDALWriter.h
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);
}

28
corelib/include/rtabmap/core/util3d.h
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(

223
corelib/src/PDALWriter.cpp
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);

6
corelib/src/Rtabmap.cpp
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

316
corelib/src/util3d.cpp
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);