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

Added util3d::projectCloudToCamera() method (creating a registered depth image from the laser scan), used for "Generate depth from scan" option of CameraImages

Browse Source
This commit is contained in:
matlabbe
2016-01-11 17:21:06 -05:00
parent 94adc6fa1b
commit 42e1590abe
12 changed files with 696 additions and 296 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/CameraInfo.h
View File

@@ -55,6 +55,7 @@ public:
float timeMirroring;
float timeImageDecimation;
float timeScanFromDepth;
float timeDepthFromScan;
};
} // namespace rtabmap

11
corelib/include/rtabmap/core/CameraRGB.h
View File

@@ -96,6 +96,13 @@ public:
}
}
void setDepthFromScan(bool enabled, bool fillHolesVertical = true, bool fillHolesFromBorder = false)
{
_depthFromScan = enabled;
_depthFromScanFillHolesVertical = fillHolesVertical;
_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
}
void setGroundTruthPath(const std::string & filePath, int format = 0)
{
groundTruthPath_ = filePath;
@@ -134,6 +141,10 @@ private:
float _scanVoxelSize;
int _scanNormalsK;
bool _depthFromScan;
bool _depthFromScanFillHolesVertical;
bool _depthFromScanFillHolesFromBorder;
bool _filenamesAreTimestamps;
std::string timestampsPath_;

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -397,7 +397,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, VoxelSize, float, 0.025, "Uniform sampling voxel size (0=disabled).");
RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling.");
RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(Icp, Iterations, int, 10, "Max iterations.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");

4
corelib/include/rtabmap/core/util2d.h
View File

@@ -109,7 +109,7 @@ float RTABMAP_EXP getDepth(
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
// Registration Depth to RGB
// Registration Depth to RGB (return registered depth image)
cv::Mat RTABMAP_EXP registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
@@ -117,7 +117,7 @@ cv::Mat RTABMAP_EXP registerDepth(
const rtabmap::Transform & transform);
void RTABMAP_EXP fillRegisteredDepthHoles(
cv::Mat & depth,
cv::Mat & depthRegistered,
bool vertical,
bool horizontal,
bool fillDoubleHoles = false);

26
corelib/include/rtabmap/core/util3d.h
View File

@@ -145,6 +145,26 @@ cv::Point3f RTABMAP_EXP projectDisparityTo3D(
const cv::Mat & disparity,
const StereoCameraModel & model);
// Register point cloud to camera (return registered depth image)
cv::Mat RTABMAP_EXP projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform);
// Register point cloud to camera (return registered depth image)
cv::Mat RTABMAP_EXP projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform);
// Direction vertical (>=0), horizontal (<0)
void RTABMAP_EXP fillProjectedCloudHoles(
cv::Mat & depthRegistered,
bool verticalDirection,
bool fillToBorder);
bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
@@ -199,6 +219,12 @@ cv::Mat RTABMAP_EXP loadScan(
float voxelSize = 0.0f,
int normalsK = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
const std::string & path,
const Transform & transform = Transform::getIdentity(),
int downsampleStep = 1,
float voxelSize = 0.0f);
} // namespace util3d
} // namespace rtabmap

206
corelib/src/CameraRGB.cpp
View File

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <iostream>
#include <cmath>
@@ -64,6 +65,7 @@ CameraImages::CameraImages() :
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_depthFromScan(false),
_filenamesAreTimestamps(false),
_groundTruthFormat(0)
{}
@@ -85,6 +87,7 @@ CameraImages::CameraImages(const std::string & path,
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_depthFromScan(false),
_filenamesAreTimestamps(false),
_groundTruthFormat(0)
{
@@ -381,6 +384,7 @@ SensorData CameraImages::captureImage()
cv::Mat scan;
double stamp = UTimer::now();
Transform groundTruthPose;
cv::Mat depthFromScan;
UDEBUG("");
if(_dir->isValid())
{
@@ -392,6 +396,8 @@ SensorData CameraImages::captureImage()
_scanDir->update();
}
}
std::string imageFilePath;
std::string scanFilePath;
if(_startAt < 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
@@ -400,8 +406,7 @@ SensorData CameraImages::captureImage()
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
std::string fullPath = _path + _lastFileName;
img = cv::imread(fullPath.c_str());
imageFilePath = _path + _lastFileName;
}
}
if(_scanDir)
@@ -412,109 +417,148 @@ SensorData CameraImages::captureImage()
if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
{
_lastScanFileName = *scanFileNames.rbegin();
std::string fullPath = _scanPath + _lastScanFileName;
scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
scanFilePath = _scanPath + _lastScanFileName;
}
}
}
}
else
{
if(stamps_.size())
{
stamp = stamps_.front();
stamps_.pop_front();
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
std::string fileName;
std::string fullPath;
fileName = _dir->getNextFileName();
if(fileName.size())
if(!fileName.empty())
{
fullPath = _path + fileName;
imageFilePath = _path + fileName;
while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
{
fullPath = _path + fileName;
}
if(fileName.size())
{
ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(fullPath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
if(_isDepth)
{
if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
{
UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
fileName.c_str());
img = cv::Mat();
}
if(_depthScaleFactor > 1.0f)
{
img /= _depthScaleFactor;
}
}
else
{
#if CV_MAJOR_VERSION < 3
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
#endif
if(img.channels()>3)
{
UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
cv::Mat out;
cv::cvtColor(img, out, CV_BGRA2BGR);
img = out;
}
}
imageFilePath = _path + fileName;
}
}
if(_scanDir)
{
fileName = _scanDir->getNextFileName();
if(fileName.size())
if(!fileName.empty())
{
fullPath = _scanPath + fileName;
scanFilePath = _scanPath + fileName;
while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
{
fullPath = _scanPath + fileName;
}
if(fileName.size())
{
scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
scanFilePath = _scanPath + fileName;
}
}
}
}
if(!img.empty() && _model.isValid() && _rectifyImages)
if(stamps_.size())
{
img = _model.rectifyImage(img);
stamp = stamps_.front();
stamps_.pop_front();
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
if(!imageFilePath.empty())
{
ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
#else
img = cv::imread(imageFilePath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
if(_isDepth)
{
if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
{
UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
imageFilePath.c_str());
img = cv::Mat();
}
if(_depthScaleFactor > 1.0f)
{
img /= _depthScaleFactor;
}
}
else
{
#if CV_MAJOR_VERSION < 3
// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
if(img.depth() != CV_8U)
{
// The depth should be 8U
UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
IplImage * i = cvLoadImage(fullPath.c_str());
img = cv::Mat(i, true);
cvReleaseImage(&i);
}
#endif
if(img.channels()>3)
{
UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
cv::Mat out;
cv::cvtColor(img, out, CV_BGRA2BGR);
img = out;
}
}
if(!img.empty() && _model.isValid() && _rectifyImages)
{
img = _model.rectifyImage(img);
}
}
if(!scanFilePath.empty())
{
// load without filtering
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::loadCloud(scanFilePath, _scanLocalTransform);
UDEBUG("Loaded scan=%d points", (int)cloud->size());
if(_depthFromScan && !img.empty())
{
UDEBUG("Computing depth from scan...");
if(!_model.isValid())
{
UWARN("Depth from laser scan: Camera model should be valid.");
}
else if(_isDepth)
{
UWARN("Depth from laser scan: Loading already a depth image.");
}
else
{
depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHolesVertical, _depthFromScanFillHolesFromBorder);
}
}
// filter the scan after registration
int previousSize = (int)cloud->size();
if(_scanDownsampleStep > 1 && cloud->size())
{
cloud = util3d::downsample(cloud, _scanDownsampleStep);
UDEBUG("Downsampling scan (step=%d): %d -> %d", _scanDownsampleStep, previousSize, (int)cloud->size());
}
previousSize = (int)cloud->size();
if(_scanVoxelSize > 0.0f && cloud->size())
{
cloud = util3d::voxelize(cloud, _scanVoxelSize);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
}
if(_scanNormalsK > 0 && cloud->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
scan = util3d::laserScanFromPointCloud(*cloudNormals);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
}
}
else
@@ -522,7 +566,7 @@ SensorData CameraImages::captureImage()
UWARN("Directory is not set, camera must be initialized.");
}
SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:cv::Mat(), _model, this->getNextSeqID(), stamp);
SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
return data;
}

27
corelib/src/util2d.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/core/util3d_transforms.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
@@ -1077,7 +1078,7 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
return out;
}
// Registration Depth to RGB
// Registration Depth to RGB (return registered depth image)
cv::Mat registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
@@ -1086,7 +1087,7 @@ cv::Mat registerDepth(
{
UASSERT(!transform.isNull());
UASSERT(!depth.empty());
UASSERT(depth.type() == CV_16UC1); // mm
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
@@ -1105,12 +1106,13 @@ cv::Mat registerDepth(
P4[3] = 1;
cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
bool depthInMM = depth.type() == CV_16UC1;
for(int y=0; y<depth.rows; ++y)
{
for(int x=0; x<depth.cols; ++x)
{
//filtering
float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x); // put in meter for projection
if(dz>=0.0f)
{
// Project to 3D
@@ -1126,11 +1128,22 @@ cv::Mat registerDepth(
if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
{
unsigned short z16 = z * 1000; //mm
unsigned short &zReg = registered.at<unsigned short>(dy, dx);
if(zReg == 0 || z16 < zReg)
if(depthInMM)
{
zReg = z16;
unsigned short z16 = z * 1000; //mm
unsigned short &zReg = registered.at<unsigned short>(dy, dx);
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
}
}
else
{
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
}

233
corelib/src/util3d.cpp
View File

@@ -787,7 +787,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull();
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
@@ -812,7 +812,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull();
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
@@ -976,6 +976,201 @@ cv::Point3f projectDisparityTo3D(
return cv::Point3f(bad_point, bad_point, bad_point);
}
// Register point cloud to camera (return registered depth image)
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const cv::Mat & laserScan, // assuming laser scan points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
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);
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
Transform t = cameraTransform.inverse();
int count = 0;
for(int i=0; i<laserScan.cols; ++i)
{
// Get 3D from laser scan
cv::Point3f ptScan;
if(laserScan.type() == CV_32FC2)
{
ptScan.x = laserScan.at<cv::Vec2f>(i)[0];
ptScan.y = laserScan.at<cv::Vec2f>(i)[1];
ptScan.z = 0;
}
else if(laserScan.type() == CV_32FC3)
{
ptScan.x = laserScan.at<cv::Vec3f>(i)[0];
ptScan.y = laserScan.at<cv::Vec3f>(i)[1];
ptScan.z = laserScan.at<cv::Vec3f>(i)[2];
}
else
{
ptScan.x = laserScan.at<cv::Vec6f>(i)[0];
ptScan.y = laserScan.at<cv::Vec6f>(i)[1];
ptScan.z = laserScan.at<cv::Vec6f>(i)[2];
}
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
int dy = (fy*ptScan.y)*invZ + cy;
if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
return registered;
}
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform)
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan->empty());
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
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);
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
Transform t = cameraTransform.inverse();
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;
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
int dy = (fy*ptScan.y)*invZ + cy;
if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
return registered;
}
void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
{
UASSERT(registeredDepth.type() == CV_32FC1);
if(verticalDirection)
{
// vertical, for each column
for(int x=0; x<registeredDepth.cols; ++x)
{
float valueA = 0.0f;
int indexA = -1;
for(int y=0; y<registeredDepth.rows; ++y)
{
float v = registeredDepth.at<float>(y,x);
if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
{
v = valueA;
}
if(v > 0.0f)
{
if(fillToBorder && indexA < 0)
{
indexA = 0;
valueA = v;
}
if(indexA >=0)
{
int range = y-indexA;
if(range > 1)
{
float slope = (v-valueA)/(range);
for(int k=1; k<range; ++k)
{
registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
}
}
}
valueA = v;
indexA = y;
}
}
}
}
else
{
// horizontal, for each row
for(int y=0; y<registeredDepth.rows; ++y)
{
float valueA = 0.0f;
int indexA = -1;
for(int x=0; x<registeredDepth.cols; ++x)
{
float v = registeredDepth.at<float>(y,x);
if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
{
v = valueA;
}
if(v > 0.0f)
{
if(fillToBorder && indexA < 0)
{
indexA = 0;
valueA = v;
}
if(indexA >=0)
{
int range = x-indexA;
if(range > 1)
{
float slope = (v-valueA)/(range);
for(int k=1; k<range; ++k)
{
registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
}
}
}
valueA = v;
indexA = x;
}
}
}
}
}
bool isFinite(const cv::Point3f & pt)
{
return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
@@ -1114,7 +1309,28 @@ cv::Mat loadScan(
int normalsK)
{
cv::Mat scan;
UDEBUG("Loading scan (step=%d, voxel=%f m, normalsK=%d) : %s", downsampleStep, voxelSize, normalsK, path.c_str());
UDEBUG("Loading scan (normalsK=%d) : %s", normalsK, path.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = loadCloud(path, Transform::getIdentity(), downsampleStep, voxelSize);
if(normalsK > 0 && cloud->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, transform);
}
return scan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
const std::string & path,
const Transform & transform,
int downsampleStep,
float voxelSize)
{
UASSERT(!transform.isNull());
UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
std::string fileName = UFile::getName(path);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(UFile::getExtension(fileName).compare("bin") == 0)
@@ -1141,16 +1357,11 @@ cv::Mat loadScan(
cloud = util3d::voxelize(cloud, voxelSize);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
}
if(normalsK > 0 && cloud->size())
if(transform.isIdentity())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
return cloud;
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, transform);
}
return scan;
return util3d::transformPointCloud(cloud, transform);
}
}