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

0.16.1: Added LaserScan class with new "format" field to distinguish easier between all kind of laser scans (XYZ, XYZRGB, XYZI, XYZNormal...)

Browse Source
This commit is contained in:
matlabbe
2018-02-16 19:20:54 -05:00
parent bfb3a58c01
commit d24097f73d
49 changed files with 2097 additions and 998 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -161,7 +161,7 @@ public:
void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const;
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
@@ -257,7 +257,7 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;

93
corelib/include/rtabmap/core/LaserScan.h Normal file
View File

@@ -0,0 +1,93 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
#include <rtabmap/core/Transform.h>
namespace rtabmap {
class LaserScan
{
public:
enum Format{kUnknown=0,
kXY=1,
kXYI=2,
kXYNormal=3,
kXYINormal=4,
kXYZ=5,
kXYZI=6,
kXYZRGB=7,
kXYZNormal=8,
kXYZINormal=9,
kXYZRGBNormal=10};
static int channels(Format format);
static bool isScan2d(const Format & format);
static bool isScanHasNormals(const Format & format);
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
public:
LaserScan();
LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
const cv::Mat & data() const {return data_;}
int maxPoints() const {return maxPoints_;}
float maxRange() const {return maxRange_;}
Format format() const {return format_;}
Transform localTransform() const {return localTransform_;}
bool isEmpty() const {return data_.empty();}
int size() const {return data_.cols;}
int dataType() const {return data_.type();}
bool is2d() const {return isScan2d(format_);}
bool hasNormals() const {return isScanHasNormals(format_);}
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
void clear() {data_ = cv::Mat();}
private:
cv::Mat data_;
int maxPoints_;
float maxRange_;
Format format_;
Transform localTransform_;
};
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */

65
corelib/include/rtabmap/core/LaserScanInfo.h
View File

@@ -1,65 +0,0 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
class LaserScanInfo
{
public:
LaserScanInfo() :
maxPoints_(0),
maxRange_(0),
localTransform_(Transform::getIdentity())
{
}
LaserScanInfo(int maxPoints, float maxRange, const Transform & localTransform = Transform::getIdentity()) :
maxPoints_(maxPoints),
maxRange_(maxRange),
localTransform_(localTransform)
{
UASSERT(!localTransform.isNull());
}
int maxPoints() const {return maxPoints_;}
float maxRange() const {return maxRange_;}
Transform localTransform() const {return localTransform_;}
private:
int maxPoints_;
float maxRange_;
Transform localTransform_;
};
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_ */

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

@@ -46,7 +46,7 @@ public:
float getCellSize() const {return cellSize_;}
void setCloudAssembling(bool enabled);
float getMinMapSize() const {return minMapSize_;}
bool isGridFromDepth() const {return occupancyFromCloud_;}
bool isGridFromDepth() const {return occupancyFromDepth_;}
bool isFullUpdate() const {return fullUpdate_;}
float getUpdateError() const {return updateError_;}
bool isMapFrameProjection() const {return projMapFrame_;}
@@ -122,7 +122,7 @@ private:
int scanDecimation_;
float cellSize_;
bool preVoxelFiltering_;
bool occupancyFromCloud_;
bool occupancyFromDepth_;
bool projMapFrame_;
float maxObstacleHeight_;
int normalKSearch_;

3
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/RegistrationInfo.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/LaserScan.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
@@ -113,7 +114,7 @@ public:
// F2M
std::multimap<int, cv::KeyPoint> words;
std::map<int, cv::Point3f> localMap;
cv::Mat localScanMap;
LaserScan localScanMap;
// F2F
std::vector<cv::Point2f> refCorners;

34
corelib/include/rtabmap/core/SensorData.h
View File

@@ -33,10 +33,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/LaserScanInfo.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/LaserScan.h>
namespace rtabmap
{
@@ -76,8 +76,7 @@ public:
// RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
@@ -96,8 +95,7 @@ public:
// Multi-cameras RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
@@ -116,8 +114,7 @@ public:
// Stereo constructor + laser scan
SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
@@ -134,8 +131,8 @@ public:
_imageCompressed.empty() &&
_depthOrRightRaw.empty() &&
_depthOrRightCompressed.empty() &&
_laserScanRaw.empty() &&
_laserScanCompressed.empty() &&
_laserScanRaw.isEmpty() &&
_laserScanCompressed.isEmpty() &&
_cameraModels.size() == 0 &&
!_stereoCameraModel.isValidForProjection() &&
_userDataRaw.empty() &&
@@ -148,18 +145,17 @@ public:
void setId(int id) {_id = id;}
double stamp() const {return _stamp;}
void setStamp(double stamp) {_stamp = stamp;}
const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
const cv::Mat & imageCompressed() const {return _imageCompressed;}
const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;}
const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
const cv::Mat & imageRaw() const {return _imageRaw;}
const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
const LaserScan & laserScanRaw() const {return _laserScanRaw;}
void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
void setLaserScanRaw(const LaserScan & laserScanRaw) {_laserScanRaw =laserScanRaw;}
void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -172,7 +168,7 @@ public:
void uncompressData(
cv::Mat * imageRaw,
cv::Mat * depthOrRightRaw,
cv::Mat * laserScanRaw = 0,
LaserScan * laserScanRaw = 0,
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0,
@@ -180,7 +176,7 @@ public:
void uncompressDataConst(
cv::Mat * imageRaw,
cv::Mat * depthOrRightRaw,
cv::Mat * laserScanRaw = 0,
LaserScan * laserScanRaw = 0,
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0,
@@ -238,7 +234,7 @@ public:
const GPS & gps() const {return gps_;}
long getMemoryUsed() const; // Return memory usage in Bytes
void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed=cv::Mat(); _userDataCompressed=cv::Mat();}
void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
@@ -248,17 +244,15 @@ private:
cv::Mat _imageCompressed; // compressed image
cv::Mat _depthOrRightCompressed; // compressed image
cv::Mat _laserScanCompressed; // compressed data
LaserScan _laserScanCompressed; // compressed data
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _laserScanRaw; // CV_32FC2 or CV_32FC3
LaserScan _laserScanRaw;
std::vector<CameraModel> _cameraModels;
StereoCameraModel _stereoCameraModel;
LaserScanInfo _laserScanInfo;
// user data
cv::Mat _userDataCompressed; // compressed data
cv::Mat _userDataRaw;

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

@@ -136,9 +136,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(TimingMem, Add_new_words, ms);
RTABMAP_STATS(TimingMem, Compressing_data, ms);
RTABMAP_STATS(TimingMem, Post_decimation, ms);
RTABMAP_STATS(TimingMem, Scan_downsampling, ms);
RTABMAP_STATS(TimingMem, Scan_voxel_filtering, ms);
RTABMAP_STATS(TimingMem, Scan_normals, ms);
RTABMAP_STATS(TimingMem, Scan_filtering, ms);
RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
RTABMAP_STATS(Keypoint, Dictionary_size, words);

5
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -83,8 +83,6 @@ void segmentObstaclesFromGround(
normalKSearch,
viewPoint);
UDEBUG("cloud=%d, indices=%d flatSurfaces=%d", (int)cloud->size(), (int)indices->size(), (int)flatSurfaces->size());
if(segmentFlatObstacles && flatSurfaces->size())
{
int biggestFlatSurfaceIndex;
@@ -95,7 +93,6 @@ void segmentObstaclesFromGround(
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
UDEBUG("clusteredFlatSurfaces=%d", (int)clusteredFlatSurfaces.size());
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size())
@@ -139,8 +136,6 @@ void segmentObstaclesFromGround(
ground = flatSurfaces;
}
UDEBUG("ground=%d", (int)ground->size());
if(ground->size() != cloud->size())
{
// Remove ground

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

@@ -196,6 +196,7 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
float maxDepth,
float minDepth);
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud);
// return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
@@ -204,34 +205,55 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
// return CV_32FC4 (x,y,z,I)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,I,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform());
// return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC3 (x,y,I)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform());
// return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// return CV_32FC6 (x,y,I,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
// For 2d laserScan, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
// For laserScan without normals, normals are set to null.
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform = Transform());
// For laserScan without rgb, rgb is set to default r,g,b parameters.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan without intensity, intensity is set to intensity parameter.
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
// For laserScan without rgb, rgb is set to default r,g,b parameters.
// For laserScan without normals, normals are set to null.
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan without intensity, intensity is set to default intensity parameter.
// For laserScan without normals, normals are set to null.
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform = Transform(), float intensity = 0.0f);
// For 2d laserScan, z is set to null.
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan & laserScan, int index);
// For laserScan without normals, normals are set to null.
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const LaserScan & laserScan, int index);
// For laserScan without rgb, rgb is set to default r,g,b parameters.
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan without intensity, intensity is set to intensity parameter.
pcl::PointXYZI RTABMAP_EXP laserScanToPointI(const LaserScan & laserScan, int index, float intensity);
// For laserScan without rgb, rgb is set to default r,g,b parameters.
// For laserScan without normals, normals are set to null.
pcl::PointXYZRGBNormal RTABMAP_EXP laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b);
// For laserScan without intensity, intensity is set to default intensity parameter.
// For laserScan without normals, normals are set to null.
pcl::PointXYZINormal RTABMAP_EXP laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity);
void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max);
void RTABMAP_EXP getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max);
@@ -316,22 +338,22 @@ void RTABMAP_EXP savePCDWords(
const std::multimap<int, cv::Point3f> & words,
const Transform & transform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
/**
* Assume KITTI velodyne format
* Return scan 4 channels (format=XYZI).
*/
cv::Mat RTABMAP_EXP loadBINScan(const std::string & fileName);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName);
RTABMAP_DEPRECATED(pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim), "Use interface without dim argument.");
// Load *.pcd, *.ply or *.bin (KITTI format) with optional filtering.
// If normals are computed (normalsK>0), the returned scan type is CV_32FC6 instead of CV_32FC3
cv::Mat RTABMAP_EXP loadScan(
// Load *.pcd, *.ply or *.bin (KITTI format).
LaserScan RTABMAP_EXP loadScan(const std::string & path);
RTABMAP_DEPRECATED(pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
const std::string & path,
const Transform & transform = Transform::getIdentity(),
int downsampleStep = 1,
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);
float voxelSize = 0.0f), "Use loadScan() instead.");
} // namespace util3d
} // namespace rtabmap

33
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -30,11 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/core/Transform.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <pcl/ModelCoefficients.h>
#include <rtabmap/core/LaserScan.h>
namespace rtabmap
{
@@ -42,13 +42,29 @@ namespace rtabmap
namespace util3d
{
cv::Mat RTABMAP_EXP rangeFiltering(
const cv::Mat & scan,
/**
* Do some filtering approaches and try to
* avoid converting between pcl and opencv and to avoid not needed
* operations like computing normals while the scan has already
* normals and voxel filtering is not used.
*/
void RTABMAP_EXP commonFiltering(
LaserScan & scan,
int downsamplingStep,
float rangeMin = 0.0f,
float rangeMax = 0.0f,
float voxelSize = 0.0f,
int normalK = 0,
float normalRadius = 0.0f,
bool forceGroundNormalsUp = false);
LaserScan RTABMAP_EXP rangeFiltering(
const LaserScan & scan,
float rangeMin,
float rangeMax);
cv::Mat RTABMAP_EXP downsample(
const cv::Mat & cloud,
LaserScan RTABMAP_EXP downsample(
const LaserScan & cloud,
int step);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -79,6 +95,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
@@ -91,6 +111,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize);
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
float voxelSize);
inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

28
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/ProgressState.h>
#include <rtabmap/core/LaserScan.h>
#include <set>
#include <list>
@@ -226,6 +227,11 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -238,17 +244,33 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -263,7 +285,7 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
float RTABMAP_EXP computeNormalsComplexity(
const cv::Mat & scan,
const LaserScan & scan,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
float RTABMAP_EXP computeNormalsComplexity(
@@ -304,6 +326,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp);
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),

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

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/LaserScan.h>
namespace rtabmap
{
@@ -41,15 +42,15 @@ namespace rtabmap
namespace util3d
{
cv::Mat RTABMAP_EXP transformLaserScan(
const cv::Mat & laserScan,
LaserScan RTABMAP_EXP transformLaserScan(
const LaserScan & laserScan,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -60,13 +61,16 @@ pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
@@ -81,6 +85,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform);
cv::Point3f RTABMAP_EXP transformPoint(
const cv::Point3f & pt,
@@ -88,6 +96,9 @@ cv::Point3f RTABMAP_EXP transformPoint(
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform);
pcl::PointXYZI RTABMAP_EXP transformPoint(
const pcl::PointXYZI & pt,
const Transform & transform);
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform);
@@ -97,6 +108,9 @@ pcl::PointNormal RTABMAP_EXP transformPoint(
pcl::PointXYZRGBNormal RTABMAP_EXP transformPoint(
const pcl::PointXYZRGBNormal & point,
const Transform & transform);
pcl::PointXYZINormal RTABMAP_EXP transformPoint(
const pcl::PointXYZINormal & point,
const Transform & transform);
} // namespace util3d
} // namespace rtabmap

1
corelib/src/CMakeLists.txt
View File

@@ -46,6 +46,7 @@ SET(SRC_FILES
Graph.cpp
Compression.cpp
Link.cpp
LaserScan.cpp
Optimizer.cpp
OptimizerTORO.cpp

45
corelib/src/CameraRGB.cpp
View File

@@ -529,12 +529,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
_captureDelay = 0.0;
cv::Mat img;
cv::Mat scan;
LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
double stamp = UTimer::now();
Transform odometryPose;
Transform groundTruthPose;
cv::Mat depthFromScan;
int scanMaxPts = _scanMaxPts;
UDEBUG("");
if(_dir->isValid())
{
@@ -729,8 +728,9 @@ SensorData CameraImages::captureImage(CameraInfo * info)
if(!scanFilePath.empty())
{
// load without filtering
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::loadCloud(scanFilePath, _scanLocalTransform);
UDEBUG("Loaded scan=%d points", (int)cloud->size());
scan = util3d::loadScan(scanFilePath);
scan = LaserScan(scan.data(), _scanMaxPts, 0.0f, scan.format(), _scanLocalTransform);
UDEBUG("Loaded scan=%d points", (int)scan.size());
if(_depthFromScan && !img.empty())
{
UDEBUG("Computing depth from scan...");
@@ -744,6 +744,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
if(_depthFromScanFillHoles!=0)
{
@@ -752,39 +753,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
// filter the scan after registration
int previousSize = (int)cloud->size();
if(_scanDownsampleStep > 1 && cloud->size())
{
cloud = util3d::downsample(cloud, _scanDownsampleStep);
int scanMaxPtsTmp = scanMaxPts;
scanMaxPts/=_scanDownsampleStep;
UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", _scanDownsampleStep, previousSize, (int)cloud->size(), scanMaxPtsTmp, scanMaxPts);
}
previousSize = (int)cloud->size();
if(_scanVoxelSize > 0.0f && cloud->size())
{
cloud = util3d::voxelize(cloud, _scanVoxelSize);
float ratio = float(cloud->size()) / previousSize;
int scanMaxPtsTmp = scanMaxPts;
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", _scanVoxelSize, previousSize, (int)cloud->size(), scanMaxPtsTmp, scanMaxPts);
}
if((_scanNormalsK > 0 || _scanNormalsRadius) && cloud->size())
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
if(_scanForceGroundNormalsUp)
{
util3d::adjustNormalsToViewPoint(cloudNormals, Eigen::Vector3f(0,0,0), _scanForceGroundNormalsUp);
}
scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
UDEBUG("Normals computed (k=%d radius=%f)", _scanNormalsK, _scanNormalsRadius);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform.inverse());
}
util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
}
}
else
@@ -797,7 +766,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
_model.setImageSize(img.size());
}
SensorData data(scan, LaserScanInfo(scan.empty()?0:scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
if(info && !odometryPose.isNull())

2
corelib/src/CameraStereo.cpp
View File

@@ -1255,7 +1255,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
stereoModel_.setImageSize(leftImage.size());
}
data = SensorData(left.laserScanRaw(), left.laserScanInfo(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
data.setGroundTruth(left.groundTruth());
}
}

6
corelib/src/CameraThread.cpp
View File

@@ -325,7 +325,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
!data.depthRaw().empty())
{
UDEBUG("");
if(data.laserScanRaw().empty())
if(data.laserScanRaw().size())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
@@ -339,6 +339,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
LaserScan::Format format = LaserScan::kXYZRGB;
if(validIndices->size())
{
if(_scanVoxelSize>0.0f)
@@ -363,6 +364,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
format = LaserScan::kXYZRGBNormal;
}
else
{
@@ -370,7 +372,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
}
}
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanMaxDepth, format, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else

6
corelib/src/DBDriver.cpp
View File

@@ -642,7 +642,7 @@ void DBDriver::getNodeData(
{
const Signature * s = _trashSignatures.at(signatureId);
if(!s->sensorData().imageCompressed().empty() ||
!s->sensorData().laserScanCompressed().empty() ||
!s->sensorData().laserScanCompressed().isEmpty() ||
!s->sensorData().userDataCompressed().empty() ||
s->sensorData().gridCellSize() != 0.0f ||
!s->isSaved())
@@ -693,7 +693,7 @@ bool DBDriver::getCalibration(
bool DBDriver::getLaserScanInfo(
int signatureId,
LaserScanInfo & info) const
LaserScan & info) const
{
UDEBUG("");
bool found = false;
@@ -701,7 +701,7 @@ bool DBDriver::getLaserScanInfo(
_trashesMutex.lock();
if(uContains(_trashSignatures, signatureId))
{
info = _trashSignatures.at(signatureId)->sensorData().laserScanInfo();
info = _trashSignatures.at(signatureId)->sensorData().laserScanCompressed();
found = true;
}
_trashesMutex.unlock();

97
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1496,6 +1496,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
int laserScanMaxPts = 0;
float laserScanMaxRange = 0.0f;
int laserScanFormat = 0;
Transform scanLocalTransform = Transform::getIdentity();
if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
{
@@ -1508,7 +1509,22 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (scanLocalTransform.size()+3)*sizeof(float))
{
// new in 0.16.1
laserScanFormat = (int)dataFloat[2];
memcpy(scanLocalTransform.data(), dataFloat+3, scanLocalTransform.size()*sizeof(float));
}
else if(dataSize == (scanLocalTransform.size()+2)*sizeof(float))
{
memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
}
else
{
UFATAL("Unexpected size %d for laser scan info!", dataSize);
}
if(uStrNumCmp(_version, "0.15.2") < 0)
{
scanLocalTransform.normalizeRotation();
@@ -1609,8 +1625,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(models.size())
{
(*iter)->sensorData() = SensorData(
scan?scanCompressed:tmp.laserScanCompressed(),
scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
scan?LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform):tmp.laserScanCompressed(),
images?imageCompressed:tmp.imageCompressed(),
images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
images?models:tmp.cameraModels(),
@@ -1621,8 +1636,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
else
{
(*iter)->sensorData() = SensorData(
scan?scanCompressed:tmp.laserScanCompressed(),
scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
scan?LaserScan(scanCompressed, laserScanMaxPts, laserScanMaxRange, (LaserScan::Format)laserScanFormat, scanLocalTransform):tmp.laserScanCompressed(),
images?imageCompressed:tmp.imageCompressed(),
images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
images?stereoModel:tmp.stereoCameraModel(),
@@ -1855,7 +1869,7 @@ bool DBDriverSqlite3::getCalibrationQuery(
bool DBDriverSqlite3::getLaserScanInfoQuery(
int signatureId,
LaserScanInfo & info) const
LaserScan & info) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -1884,6 +1898,7 @@ bool DBDriverSqlite3::getLaserScanInfoQuery(
Transform localTransform = Transform::getIdentity();
int maxPts = 0;
float maxRange = 0.0f;
int format = 0;
// Process the result if one
rc = sqlite3_step(ppStmt);
@@ -1899,7 +1914,20 @@ bool DBDriverSqlite3::getLaserScanInfoQuery(
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.16.1") >= 0 && dataSize == (localTransform.size()+3)*sizeof(float))
{
// new in 0.16.1
format = (int)dataFloat[2];
memcpy(localTransform.data(), dataFloat+3, localTransform.size()*sizeof(float));
}
else if(dataSize == (localTransform.size()+2)*sizeof(float))
{
memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
}
else
{
UFATAL("Unexpected size %d for laser scan info!", dataSize);
}
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
@@ -1907,7 +1935,7 @@ bool DBDriverSqlite3::getLaserScanInfoQuery(
maxPts = (int)dataFloat[0];
maxRange = dataFloat[1];
info = LaserScanInfo(maxPts, maxRange, localTransform);
info = LaserScan(cv::Mat(), maxPts, maxRange, (LaserScan::Format)format, localTransform);
}
rc = sqlite3_step(ppStmt); // next result...
@@ -3676,7 +3704,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
// raw data are not kept in database
_memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().total() * (*i)->sensorData().laserScanRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
stepNode(ppStmt, *i);
}
@@ -3755,7 +3783,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
{
if(!(*i)->sensorData().imageCompressed().empty() ||
!(*i)->sensorData().depthOrRightCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().isEmpty() ||
!(*i)->sensorData().userDataCompressed().empty() ||
!(*i)->sensorData().cameraModels().size() ||
!(*i)->sensorData().stereoCameraModel().isValidForProjection())
@@ -3798,7 +3826,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
//metric
if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty())
if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().isEmpty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepDepth(ppStmt, (*i)->sensorData());
@@ -4741,7 +4769,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
UDEBUG("Save depth %d (size=%d) depth2d = %d",
sensorData.id(),
(int)sensorData.depthOrRightCompressed().cols,
(int)sensorData.laserScanCompressed().cols);
sensorData.laserScanCompressed().size());
if(!ppStmt)
{
UFATAL("");
@@ -4805,9 +4833,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(!sensorData.laserScanCompressed().empty())
if(!sensorData.laserScanCompressed().isEmpty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, (int)sensorData.laserScanCompressed().size(), SQLITE_STATIC);
}
else
{
@@ -4817,7 +4845,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().maxPoints());
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
@@ -4914,7 +4942,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
sensorData.id(),
(int)sensorData.imageCompressed().cols,
(int)sensorData.depthOrRightCompressed().cols,
(int)sensorData.laserScanCompressed().cols);
sensorData.laserScanCompressed().size());
if(!ppStmt)
{
UFATAL("");
@@ -5013,15 +5041,28 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
std::vector<float> scanInfo;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
if(sensorData.laserScanInfo().maxPoints() > 0 ||
sensorData.laserScanInfo().maxRange() > 0 ||
(!sensorData.laserScanInfo().localTransform().isNull() && !sensorData.laserScanInfo().localTransform().isIdentity()))
if(sensorData.laserScanCompressed().maxPoints() > 0 ||
sensorData.laserScanCompressed().maxRange() > 0 ||
(uStrNumCmp(_version, "0.16.1")>=0 && sensorData.laserScanCompressed().format() != LaserScan::kUnknown) ||
(!sensorData.laserScanCompressed().localTransform().isNull() && !sensorData.laserScanCompressed().localTransform().isIdentity()))
{
scanInfo.resize(2 + Transform().size());
scanInfo[0] = sensorData.laserScanInfo().maxPoints();
scanInfo[1] = sensorData.laserScanInfo().maxRange();
const Transform & localTransform = sensorData.laserScanInfo().localTransform();
memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.16.1") >=0)
{
scanInfo.resize(3 + Transform().size());
scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
scanInfo[1] = sensorData.laserScanCompressed().maxRange();
scanInfo[2] = sensorData.laserScanCompressed().format();
const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
}
else
{
scanInfo.resize(2 + Transform().size());
scanInfo[0] = sensorData.laserScanCompressed().maxPoints();
scanInfo[1] = sensorData.laserScanCompressed().maxRange();
const Transform & localTransform = sensorData.laserScanCompressed().localTransform();
memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
}
}
if(scanInfo.size())
@@ -5038,21 +5079,21 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
else
{
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().maxPoints());
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanCompressed().maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// scan_max_range
if(uStrNumCmp(_version, "0.10.7") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanInfo().maxRange());
rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanCompressed().maxRange());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// scan
if(!sensorData.laserScanCompressed().empty())
if(!sensorData.laserScanCompressed().isEmpty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data().data, sensorData.laserScanCompressed().size(), SQLITE_STATIC);
}
else
{

2
corelib/src/DBDriverSqlite3.h
View File

@@ -129,7 +129,7 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const;
virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;

2
corelib/src/DBReader.cpp
View File

@@ -443,7 +443,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.setGroundTruth(groundTruth);
data.setGPS(gps);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.laserScanRaw().isEmpty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.userDataRaw().empty()?0:1);

152
corelib/src/LaserScan.cpp Normal file
View File

@@ -0,0 +1,152 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/LaserScan.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
int LaserScan::channels(Format format)
{
int channels=0;
switch (format) {
case kXY:
channels = 2;
break;
case kXYZ:
case kXYI:
channels = 3;
break;
case kXYZI:
case kXYZRGB:
channels = 4;
break;
case kXYNormal:
channels = 5;
break;
case kXYZNormal:
case kXYINormal:
channels = 6;
break;
case kXYZINormal:
case kXYZRGBNormal:
channels = 7;
break;
default:
UFATAL("Unhandled type %d!", (int)format);
break;
}
return channels;
}
bool LaserScan::isScan2d(const Format & format)
{
return format==kXY || format==kXYI || format == kXYNormal || format == kXYINormal;
}
bool LaserScan::isScanHasNormals(const Format & format)
{
return format==kXYZNormal || format==kXYZINormal || format==kXYZRGBNormal || format == kXYNormal || format == kXYINormal;
}
bool LaserScan::isScanHasRGB(const Format & format)
{
return format==kXYZRGB || format==kXYZRGBNormal;
}
bool LaserScan::isScanHasIntensity(const Format & format)
{
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
}
LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints, int maxRange, const Transform & localTransform)
{
if(!oldScanFormat.empty())
{
if(oldScanFormat.channels() == 2)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXY, localTransform);
}
else if(oldScanFormat.channels() == 3)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZ, localTransform);
}
else if(oldScanFormat.channels() == 4)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGB, localTransform);
}
else if(oldScanFormat.channels() == 5)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXYNormal, localTransform);
}
else if(oldScanFormat.channels() == 6)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZNormal, localTransform);
}
else if(oldScanFormat.channels() == 7)
{
return LaserScan(oldScanFormat, maxPoints, maxRange, kXYZRGBNormal, localTransform);
}
}
return LaserScan();
}
LaserScan::LaserScan() :
maxPoints_(0),
maxRange_(0),
format_(kUnknown),
localTransform_(Transform::getIdentity())
{
}
LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform) :
data_(data),
maxPoints_(maxPoints),
maxRange_(maxRange),
format_(format),
localTransform_(localTransform)
{
UASSERT(data.empty() || data.rows == 1);
UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(!localTransform.isNull());
if(!data.empty() && !isCompressed())
{
if(format == kUnknown)
{
*this = backwardCompatibility(data_, maxPoints_, maxRange_, localTransform_);
}
else // verify that format corresponds to expected number of channels
{
UASSERT(data.channels() != 2 || (data.channels() == 2 && format == kXY));
UASSERT(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)));
UASSERT(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)));
UASSERT(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)));
UASSERT(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)));
UASSERT(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)));
}
}
}
}

184
corelib/src/Memory.cpp
View File

@@ -2261,7 +2261,9 @@ void Memory::removeRawData(int id, bool image, bool scan, bool userData)
}
if(scan && !_registrationPipeline->isScanRequired())
{
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanInfo());
LaserScan scan = s->sensorData().laserScanRaw();
scan.clear();
s->sensorData().setLaserScanRaw(scan);
}
if(userData && !_registrationPipeline->isUserDataRequired())
{
@@ -2312,19 +2314,20 @@ Transform Memory::computeTransform(
// make sure we have all data needed
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
(_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
{
fromS.sensorData() = getNodeData(fromS.id());
}
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
(_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
{
toS.sensorData() = getNodeData(toS.id());
}
// uncompress only what we need
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
cv::Mat imgBuf, depthBuf, userBuf;
LaserScan laserBuf;
fromS.sensorData().uncompressData(
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
@@ -2422,12 +2425,12 @@ Transform Memory::computeIcpTransform(
std::list<Signature*> depthsToLoad;
//if image is already here, scan should be or it is null
if(fromS->sensorData().imageCompressed().empty() &&
fromS->sensorData().laserScanCompressed().empty())
fromS->sensorData().laserScanCompressed().isEmpty())
{
depthsToLoad.push_back(fromS);
}
if(toS->sensorData().imageCompressed().empty() &&
toS->sensorData().laserScanCompressed().empty())
toS->sensorData().laserScanCompressed().isEmpty())
{
depthsToLoad.push_back(toS);
}
@@ -2442,7 +2445,7 @@ Transform Memory::computeIcpTransform(
if(fromS && toS)
{
//make sure data are uncompressed
cv::Mat tmp1, tmp2;
LaserScan tmp1, tmp2;
fromS->sensorData().uncompressData(0, 0, &tmp1);
toS->sensorData().uncompressData(0, 0, &tmp2);
@@ -2494,7 +2497,7 @@ Transform Memory::computeIcpTransformMulti(
UASSERT(s != 0);
//if image is already here, scan should be or it is null
if(s->sensorData().imageCompressed().empty() &&
s->sensorData().laserScanCompressed().empty())
s->sensorData().laserScanCompressed().isEmpty())
{
depthToLoad.push_back(s);
}
@@ -2505,54 +2508,71 @@ Transform Memory::computeIcpTransformMulti(
}
Signature * fromS = _getSignature(fromId);
cv::Mat fromScan;
LaserScan fromScan;
fromS->sensorData().uncompressData(0, 0, &fromScan);
Transform t;
if(!fromScan.empty())
if(!fromScan.isEmpty())
{
// Create a fake signature with all scans merged in oldId referential
SensorData assembledData;
Transform toPoseInv = poses.at(toId).inverse();
std::string msg;
int maxPoints = fromScan.cols;
int maxPoints = fromScan.size();
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
{
Signature * s = this->_getSignature(iter->first);
if(!s->sensorData().laserScanCompressed().empty())
if(!s->sensorData().laserScanCompressed().isEmpty())
{
cv::Mat scan;
LaserScan scan;
s->sensorData().uncompressData(0, 0, &scan);
if(!scan.empty())
if(!scan.isEmpty() && scan.format() == fromS->sensorData().laserScanRaw().format())
{
if(scan.channels() != 2 && scan.channels() != 5)
{
is2D = false;
}
is2D = !scan.is2d();
if(scan.channels() >= 5)
if(scan.hasIntensity())
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(
scan,
toPoseInv * iter->second * s->sensorData().laserScanInfo().localTransform());
if(scan.hasNormals())
{
*assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
}
else
{
*assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
}
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(
scan,
toPoseInv * iter->second * s->sensorData().laserScanInfo().localTransform());
if(scan.hasNormals())
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
}
}
if(scan.cols > maxPoints)
if(scan.size() > maxPoints)
{
maxPoints = scan.cols;
maxPoints = scan.size();
}
}
else if(!scan.isEmpty())
{
UWARN("Incompatible scan format %d vs %d", (int)fromS->sensorData().laserScanRaw().format(), (int)scan.format());
}
}
else
{
@@ -2570,12 +2590,21 @@ Transform Memory::computeIcpTransformMulti(
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
else if(assembledToNormalIClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
}
else if(assembledToIClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(assembledScan,
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
assembledData.setLaserScanRaw(
LaserScan(assembledScan,
fromS->sensorData().laserScanRaw().maxPoints()?fromS->sensorData().laserScanRaw().maxPoints():maxPoints,
fromS->sensorData().laserScanRaw().maxRange(),
fromS->sensorData().laserScanRaw().format(),
is2D?Transform(0,0,fromS->sensorData().laserScanRaw().localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
@@ -3265,7 +3294,7 @@ SensorData Memory::getSignatureDataConst(int locationId,
SensorData r;
const Signature * s = this->getSignature(locationId);
if(s && (!s->sensorData().imageCompressed().empty() ||
!s->sensorData().laserScanCompressed().empty() ||
!s->sensorData().laserScanCompressed().isEmpty() ||
!s->sensorData().userDataCompressed().empty() ||
s->sensorData().gridCellSize() != 0.0f))
{
@@ -3378,7 +3407,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.depthOrRightRaw().rows,
data.depthOrRightRaw().type(),
CV_16UC1, CV_32FC1, CV_8UC1).c_str());
UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(5) || data.laserScanRaw().type() == CV_32FC(6) || data.laserScanRaw().type() == CV_32FC(7));
if(!data.depthOrRightRaw().empty() &&
data.cameraModels().size() == 0 &&
@@ -3821,55 +3849,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("time post-decimation = %fs", t);
}
// downsampling the laser scan?
cv::Mat laserScan = data.laserScanRaw();
int maxLaserScanMaxPts = data.laserScanInfo().maxPoints();
if(!laserScan.empty() && _laserScanDownsampleStepSize > 1 && !isIntermediateNode)
// Filter the laser scan?
LaserScan laserScan = data.laserScanRaw();
if(!isIntermediateNode && laserScan.size())
{
laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
util3d::commonFiltering(laserScan,
_laserScanDownsampleStepSize,
0,
0,
_laserScanVoxelSize,
_laserScanNormalK,
_laserScanNormalRadius);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
UDEBUG("time downsampling scan = %fs", t);
}
if(!laserScan.empty() && _laserScanVoxelSize > 0.0f && !isIntermediateNode)
{
float pointsBeforeFiltering = laserScan.cols;
if(laserScan.channels() == 4 || laserScan.channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
laserScan = util3d::laserScanFromPointCloud(*cloud);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
if(laserScan.channels() == 2 || laserScan.channels() == 5)
{
laserScan = util3d::laserScan2dFromPointCloud(*cloud);
}
else
{
laserScan = util3d::laserScanFromPointCloud(*cloud);
}
}
float ratio = float(laserScan.cols) / pointsBeforeFiltering;
maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f);
UDEBUG("time voxel filtering scan = %fs", t);
}
if(!laserScan.empty() &&
(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
laserScan.channels() > 1 && laserScan.channels() < 5 &&
!isIntermediateNode)
{
laserScan = util3d::computeNormals(laserScan, _laserScanNormalK, _laserScanNormalRadius);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f);
if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
UDEBUG("time normals scan = %fs", t);
}
@@ -3879,7 +3871,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
image.empty()?0:1,
depthOrRightImage.empty()?0:1,
laserScan.empty()?0:1,
laserScan.isEmpty()?0:1,
data.userDataRaw().empty()?0:1);
std::vector<unsigned char> imageBytes;
@@ -3899,7 +3891,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctLaserScan(laserScan);
rtabmap::CompressionThread ctLaserScan(laserScan.data());
rtabmap::CompressionThread ctUserData(data.userDataRaw());
if(!image.empty())
{
@@ -3909,7 +3901,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
ctDepth.start();
}
if(!laserScan.empty())
if(!laserScan.isEmpty())
{
ctLaserScan.start();
}
@@ -3931,7 +3923,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
compressedImage = compressImage2(image, std::string(".jpg"));
compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):std::string(".jpg"));
compressedScan = compressData2(laserScan);
compressedScan = compressData2(laserScan.data());
compressedUserData = compressData2(data.userDataRaw());
}
@@ -3944,8 +3936,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
SensorData(
compressedScan,
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
LaserScan(compressedScan, data.laserScanRaw().maxPoints(), data.laserScanRaw().maxRange(), data.laserScanRaw().format(), data.laserScanRaw().localTransform()),
compressedImage,
compressedDepth,
stereoCameraModel,
@@ -3953,8 +3944,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
0,
compressedUserData):
SensorData(
compressedScan,
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
LaserScan(compressedScan, data.laserScanRaw().maxPoints(), data.laserScanRaw().maxRange(), data.laserScanRaw().format(), data.laserScanRaw().localTransform()),
compressedImage,
compressedDepth,
cameraModels,
@@ -3965,7 +3955,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
else
{
UDEBUG("Bin data kept: scan=%d, userData=%d",
laserScan.empty()?0:1,
laserScan.isEmpty()?0:1,
data.userDataRaw().empty()?0:1);
// just compress user data and laser scan (scans can be used for local scan matching)
@@ -3974,12 +3964,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(_compressionParallelized)
{
rtabmap::CompressionThread ctUserData(data.userDataRaw());
rtabmap::CompressionThread ctLaserScan(laserScan);
rtabmap::CompressionThread ctLaserScan(laserScan.data());
if(!data.userDataRaw().empty() && !isIntermediateNode)
{
ctUserData.start();
}
if(!laserScan.empty() && !isIntermediateNode)
if(!laserScan.isEmpty() && !isIntermediateNode)
{
ctLaserScan.start();
}
@@ -3991,7 +3981,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
else
{
compressedScan = compressData2(laserScan);
compressedScan = compressData2(laserScan.data());
compressedUserData = compressData2(data.userDataRaw());
}
@@ -4004,8 +3994,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
SensorData(
compressedScan,
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
LaserScan(compressedScan, data.laserScanRaw().maxPoints(), data.laserScanRaw().maxRange(), data.laserScanRaw().format(), data.laserScanRaw().localTransform()),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
@@ -4013,8 +4002,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
0,
compressedUserData):
SensorData(
compressedScan,
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
LaserScan(compressedScan, data.laserScanRaw().maxPoints(), data.laserScanRaw().maxRange(), data.laserScanRaw().format(), data.laserScanRaw().localTransform()),
cv::Mat(),
cv::Mat(),
cameraModels,
@@ -4030,7 +4018,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
// set raw data
s->sensorData().setImageRaw(image);
s->sensorData().setDepthOrRightRaw(depthOrRightImage);
s->sensorData().setLaserScanRaw(laserScan, LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()));
s->sensorData().setLaserScanRaw(data.laserScanRaw());
s->sensorData().setUserDataRaw(data.userDataRaw());
s->sensorData().setGroundTruth(data.groundTruth());

44
corelib/src/OccupancyGrid.cpp
View File

@@ -52,7 +52,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
scanDecimation_(Parameters::defaultGridScanDecimation()),
cellSize_(Parameters::defaultGridCellSize()),
preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
occupancyFromCloud_(Parameters::defaultGridFromDepth()),
occupancyFromDepth_(Parameters::defaultGridFromDepth()),
projMapFrame_(Parameters::defaultGridMapFrameProjection()),
maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
normalKSearch_(Parameters::defaultGridNormalK()),
@@ -87,7 +87,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
void OccupancyGrid::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromDepth_);
Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
if(cloudDecimation_ == 0)
{
@@ -228,46 +228,46 @@ void OccupancyGrid::createLocalMap(
cv::Mat & emptyCells,
cv::Point3f & viewPoint) const
{
UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
UDEBUG("scan format=%d, occupancyFromDepth_=%d normalsSegmentation_=%d grid3D_=%d",
node.sensorData().laserScanRaw().isEmpty()?0:node.sensorData().laserScanRaw().format(), occupancyFromDepth_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
if((node.sensorData().laserScanRaw().channels() == 2 || node.sensorData().laserScanRaw().channels() == 5) && !occupancyFromCloud_)
if((node.sensorData().laserScanRaw().is2d()) && !occupancyFromDepth_)
{
UDEBUG("2D laser scan");
//2D
viewPoint = cv::Point3f(
node.sensorData().laserScanInfo().localTransform().x(),
node.sensorData().laserScanInfo().localTransform().y(),
node.sensorData().laserScanInfo().localTransform().z());
node.sensorData().laserScanRaw().localTransform().x(),
node.sensorData().laserScanRaw().localTransform().y(),
node.sensorData().laserScanRaw().localTransform().z());
cv::Mat scan = node.sensorData().laserScanRaw();
LaserScan scan = node.sensorData().laserScanRaw();
if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
{
scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
}
util3d::occupancy2DFromLaserScan(
util3d::transformLaserScan(scan, node.sensorData().laserScanInfo().localTransform()),
util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
cv::Mat(),
viewPoint,
emptyCells,
obstacleCells,
cellSize_,
scan2dUnknownSpaceFilled_,
node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
node.sensorData().laserScanRaw().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanRaw().maxRange());
UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
}
else
{
// 3D
if(!occupancyFromCloud_)
if(!occupancyFromDepth_)
{
if(!node.sensorData().laserScanRaw().empty())
if(!node.sensorData().laserScanRaw().isEmpty())
{
UDEBUG("3D laser scan");
const Transform & t = node.sensorData().laserScanInfo().localTransform();
cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
const Transform & t = node.sensorData().laserScanRaw().localTransform();
LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
{
@@ -277,12 +277,14 @@ void OccupancyGrid::createLocalMap(
// update viewpoint
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
if(scan.channels() == 6 || scan.channels() == 7)
UDEBUG("scan format=%d", scan.format());
if(scan.hasNormals())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, t);
pcl::io::savePCDFile("test.pcd", *cloud);
createLocalMap<pcl::PointXYZRGBNormal>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
else if(scan.channels() == 4)
else if(scan.hasRGB())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, t);
createLocalMap<pcl::PointXYZRGB>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
@@ -359,7 +361,7 @@ void OccupancyGrid::createLocalMapImpl(
{
if(grid3D_)
{
UDEBUG("");
UDEBUG("ground=%d obstacles=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
if(groundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
@@ -763,7 +765,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(cloudAssembling_)
{
*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first.first, iter->second, 0, 255, 0);
*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.first), iter->second, 0, 255, 0);
assembledGroundUpdated = true;
}
}
@@ -805,7 +807,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(cloudAssembling_)
{
*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 0, 255, 0);
*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.second), iter->second, 0, 255, 0);
assembledEmptyCellsUpdated = true;
}
}
@@ -847,7 +849,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(cloudAssembling_)
{
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.first.second, iter->second, 255, 0, 0);
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(pair.first.second), iter->second, 255, 0, 0);
assembledObstaclesUpdated = true;
}
}

20
corelib/src/OctoMap.cpp
View File

@@ -544,12 +544,18 @@ void OctoMap::update(const std::map<int, Transform> & poses)
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
LaserScan tmpGround;
if(occupancyIter != cache_.end())
{
tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
UASSERT(tmpGround.size() == maxGroundPts);
}
for (unsigned int i=0; i<maxGroundPts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first.first, i);
pt = util3d::laserScanToPointRGB(tmpGround, i);
pt = pcl::transformPoint(pt, t);
}
else
@@ -602,12 +608,18 @@ void OctoMap::update(const std::map<int, Transform> & poses)
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
LaserScan tmpObstacle;
if(occupancyIter != cache_.end())
{
tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
UASSERT(tmpObstacle.size() == maxObstaclePts);
}
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first.second, i);
pt = util3d::laserScanToPointRGB(tmpObstacle, i);
pt = pcl::transformPoint(pt, t);
}
else
@@ -689,10 +701,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
{
unsigned int maxEmptyPts = occupancyIter->second.second.cols;
UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
UASSERT(tmpEmpty.size() == maxEmptyPts);
for (unsigned int i=0; i<maxEmptyPts; ++i)
{
pcl::PointXYZ pt;
pt = util3d::laserScanToPoint(occupancyIter->second.second, i);
pt = util3d::laserScanToPoint(tmpEmpty, i);
pt = pcl::transformPoint(pt, t);
octomap::point3d point(pt.x, pt.y, pt.z);

16
corelib/src/OdometryF2F.cpp
View File

@@ -206,8 +206,8 @@ Transform OdometryF2F::computeTransform(
info->localMapSize = tmpRefFrame.getWords3().size();
info->words = newFrame.getWords();
info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols;
info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t*tmpRefFrame.sensorData().laserScanInfo().localTransform());
info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();
info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t);
}
}
else
@@ -245,8 +245,8 @@ Transform OdometryF2F::computeTransform(
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
(registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
(newFrame.sensorData().laserScanRaw().cols &&
(newFrame.sensorData().laserScanInfo().maxPoints() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
(newFrame.sensorData().laserScanRaw().size() &&
(newFrame.sensorData().laserScanRaw().maxPoints() == 0 || float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
{
refFrame_ = newFrame;
@@ -272,13 +272,13 @@ Transform OdometryF2F::computeTransform(
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().cols==0)
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().size()==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().size());
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanInfo().maxPoints() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().maxPoints() != 0 && float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
{
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
}
}
}

71
corelib/src/OdometryF2M.cpp
View File

@@ -70,6 +70,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
bundleSeq_(0),
sba_(0)
{
@@ -207,7 +208,7 @@ Transform OdometryF2M::computeTransform(
// Generate keypoints from the new data
if(lastFrame_->sensorData().isValid())
{
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().empty()) &&
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().isEmpty()) &&
lastFrame_->sensorData().isValid())
{
Signature tmpMap;
@@ -489,7 +490,7 @@ Transform OdometryF2M::computeTransform(
Transform newFramePose = this->getPose()*output;
// fields to update
cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
@@ -765,10 +766,10 @@ Transform OdometryF2M::computeTransform(
UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
if(lastFrame_->sensorData().laserScanRaw().cols)
if(lastFrame_->sensorData().laserScanRaw().size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
@@ -856,15 +857,15 @@ Transform OdometryF2M::computeTransform(
*mapCloudNormals += *scansBuffer_.back().first;
}
}
if(mapScan.channels() == 2 || mapScan.channels() == 5)
if(mapScan.is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
}
modified=true;
}
@@ -876,16 +877,26 @@ Transform OdometryF2M::computeTransform(
{
*map_ = tmpMap;
if(mapScan.channels() == 2 || mapScan.channels() == 5)
if(mapScan.is2d())
{
map_->sensorData().setLaserScanRaw(mapScan,
LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
map_->sensorData().setLaserScanRaw(
LaserScan(
mapScan.data(),
0,
0.0f,
mapScan.format(),
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
{
map_->sensorData().setLaserScanRaw(mapScan,
LaserScanInfo(0, 0.0f, newFramePose.translation()));
map_->sensorData().setLaserScanRaw(
LaserScan(
mapScan.data(),
0,
0.0f,
mapScan.format(),
newFramePose.translation()));
}
map_->setWords(mapWords);
@@ -898,11 +909,11 @@ Transform OdometryF2M::computeTransform(
{
// use tmpMap instead of map_ to make sure that correspondences with the new frame matches
info->localMapSize = (int)tmpMap.getWords3().size();
info->localScanMapSize = tmpMap.sensorData().laserScanRaw().cols;
info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(tmpMap.getWords3());
info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform());
info->localScanMap = tmpMap.sensorData().laserScanRaw();
}
}
}
@@ -1030,22 +1041,32 @@ Transform OdometryF2M::computeTransform(
}
if(regPipeline_->isScanRequired())
{
if (lastFrame_->sensorData().laserScanRaw().cols)
if (lastFrame_->sensorData().laserScanRaw().size())
{
frameValid = true;
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5)
if(lastFrame_->sensorData().laserScanRaw().is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
map_->sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYNormal,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
LaserScanInfo(0, 0.0f, newFramePose.translation()));
map_->sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
LaserScan::kXYZNormal,
newFramePose.translation()));
}
addKeyFrame = true;
}
@@ -1064,12 +1085,12 @@ Transform OdometryF2M::computeTransform(
if(info)
{
info->localMapSize = (int)map_->getWords3().size();
info->localScanMapSize = map_->sensorData().laserScanRaw().cols;
info->localScanMapSize = map_->sensorData().laserScanRaw().size();
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(map_->getWords3());
info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform());
info->localScanMap = map_->sensorData().laserScanRaw();
}
}
}
@@ -1114,7 +1135,7 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance.at<double>(0,0),
regInfo.covariance.at<double>(5,5),
regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
return output;
}

158
corelib/src/RegistrationIcp.cpp
View File

@@ -388,7 +388,7 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Voxel size=%f", _voxelSize);
UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
UDEBUG("Normal radius=%d", _pointToPlaneRadius);
UDEBUG("Normal radius=%f", _pointToPlaneRadius);
UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _maxIterations);
UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
@@ -404,33 +404,27 @@ Transform RegistrationIcp::computeTransformationImpl(
SensorData & dataFrom = fromSignature.sensorData();
SensorData & dataTo = toSignature.sensorData();
UDEBUG("size from=%d (channels=%d, max pts=%d) to=%d (channels=%d, max pts=%d)",
dataFrom.laserScanRaw().cols,
dataFrom.laserScanRaw().channels(),
dataFrom.laserScanInfo().maxPoints(),
dataTo.laserScanRaw().cols,
dataTo.laserScanRaw().channels(),
dataTo.laserScanInfo().maxPoints());
UDEBUG("size from=%d (format=%d, max pts=%d) to=%d (format=%d, max pts=%d)",
dataFrom.laserScanRaw().size(),
(int)dataFrom.laserScanRaw().format(),
dataFrom.laserScanRaw().maxPoints(),
dataTo.laserScanRaw().size(),
(int)dataTo.laserScanRaw().format(),
dataTo.laserScanRaw().maxPoints());
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
if(!guess.isNull() && !dataFrom.laserScanRaw().isEmpty() && !dataTo.laserScanRaw().isEmpty())
{
// ICP with guess transform
int maxLaserScansTo = dataTo.laserScanInfo().maxPoints();
int maxLaserScansFrom = dataFrom.laserScanInfo().maxPoints();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
Transform fromLocalTransform = dataFrom.laserScanInfo().localTransform();
Transform toLocalTransform = dataTo.laserScanInfo().localTransform();
LaserScan fromScan = dataFrom.laserScanRaw();
LaserScan toScan = dataTo.laserScanRaw();
if(_downsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _downsamplingStep);
toScan = util3d::downsample(toScan, _downsamplingStep);
maxLaserScansTo/=_downsamplingStep;
maxLaserScansFrom/=_downsamplingStep;
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
}
if(fromScan.cols && toScan.cols)
if(fromScan.size() && toScan.size())
{
Transform icpT;
bool hasConverged = false;
@@ -443,9 +437,9 @@ Transform RegistrationIcp::computeTransformationImpl(
if( _pointToPlane &&
_voxelSize == 0.0f &&
fromScan.channels() >= 5 &&
toScan.channels() >= 5 &&
!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
fromScan.hasNormals() &&
toScan.hasNormals() &&
!((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D)
{
//special case if we have already normals computed and there is no filtering
@@ -462,8 +456,8 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
@@ -475,8 +469,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
DP data = pclToDP(fromCloudNormals, fromScan.is2d());
DP ref = pclToDP(toCloudNormals, toScan.is2d());
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -531,10 +525,12 @@ Transform RegistrationIcp::computeTransformationImpl(
}
}
int maxLaserScansFrom = fromScan.maxPoints();
int maxLaserScansTo = toScan.maxPoints();
if(!transformComputed)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
@@ -565,11 +561,11 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane && // ICP Point To Plane
!tooLowComplexityForPlaneToPlane && // if previously rejected above
!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
!((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
{
Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
Eigen::Vector3f viewpointFrom(fromScan.localTransform().x(), fromScan.localTransform().y(), fromScan.localTransform().z());
pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
if(fromScan.channels() == 2 || fromScan.channels() == 5)
if(fromScan.is2d())
{
if(_voxelSize > 0.0f)
{
@@ -593,10 +589,10 @@ Transform RegistrationIcp::computeTransformationImpl(
normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
}
Transform toT = guess * toLocalTransform;
Transform toT = guess * toScan.localTransform();
Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
if(toScan.channels() == 2 || toScan.channels() == 5)
if(toScan.is2d())
{
if(_voxelSize > 0.0f)
{
@@ -621,8 +617,8 @@ Transform RegistrationIcp::computeTransformationImpl(
}
cv::Mat complexityVectorsFrom, complexityVectorsTo;
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5, &complexityVectorsFrom);
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5, &complexityVectorsTo);
double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
@@ -644,21 +640,45 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
if(fromScan.channels() == 2 || fromScan.channels() == 5)
if(fromScan.is2d())
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
fromSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
LaserScan::kXYNormal,
fromScan.localTransform()));
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
fromSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
LaserScan::kXYZNormal,
fromScan.localTransform()));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
if(toScan.is2d())
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
toSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
LaserScan::kXYNormal,
toScan.localTransform()));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
toSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
LaserScan::kXYZNormal,
toScan.localTransform()));
}
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
@@ -670,8 +690,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
DP data = pclToDP(fromCloudNormals, fromScan.is2d());
DP ref = pclToDP(toCloudNormals, toScan.is2d());
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -729,7 +749,7 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!transformComputed) // ICP Point to Point
{
if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher))
{
UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
}
@@ -737,21 +757,45 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
{
// update output scans
if(fromScan.channels() == 2 || fromScan.channels() == 5)
if(fromScan.is2d())
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
fromSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
LaserScan::kXY,
fromScan.localTransform()));
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
fromSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
maxLaserScansFrom,
fromScan.maxRange(),
LaserScan::kXYZ,
fromScan.localTransform()));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
if(toScan.is2d())
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
toSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
LaserScan::kXY,
toScan.localTransform()));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
toSignature.sensorData().setLaserScanRaw(
LaserScan(
util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
maxLaserScansTo,
toScan.maxRange(),
LaserScan::kXYZ,
toScan.localTransform()));
}
}
@@ -759,8 +803,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudFiltered, toScan.channels() == 2 || toScan.channels() == 5);
DP data = pclToDP(fromCloudFiltered, fromScan.is2d());
DP ref = pclToDP(toCloudFiltered, toScan.is2d());
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -859,8 +903,8 @@ Transform RegistrationIcp::computeTransformationImpl(
icpT = guess * t.inverse() * guessInv;
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
@@ -922,7 +966,7 @@ Transform RegistrationIcp::computeTransformationImpl(
dataTo.id());
warningShown = true;
}
correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
correspondencesRatio = float(correspondences)/float(toScan.size()>fromScan.size()?toScan.size():fromScan.size());
}
variance/=10.0;
@@ -932,7 +976,7 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged?"true":"false",
variance,
correspondences,
maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
maxLaserScans>0?maxLaserScans:(int)(toScan.size()>fromScan.size()?toScan.size():fromScan.size()),
correspondencesRatio*100.0f,
info.icpTranslation,
info.icpRotation);
@@ -974,8 +1018,8 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
dataTo.id(), dataTo.laserScanRaw().total(),
dataFrom.id(), dataFrom.laserScanRaw().total());
dataTo.id(), dataTo.laserScanRaw().size(),
dataFrom.id(), dataFrom.laserScanRaw().size());
}
UERROR(msg.c_str());
}

6
corelib/src/Rtabmap.cpp
View File

@@ -1109,7 +1109,7 @@ bool Rtabmap::process(
//============================================================
// Refine neighbor links
//============================================================
if(!signature->sensorData().laserScanCompressed().empty())
if(!signature->sensorData().laserScanCompressed().isEmpty())
{
UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info;
@@ -1156,7 +1156,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().size());
}
}
timeNeighborLinkRefining = timer.ticks();
@@ -2014,7 +2014,7 @@ bool Rtabmap::process(
//
UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
if( _proximityMaxNeighbors > 0 &&
!signature->sensorData().laserScanCompressed().empty() &&
!signature->sensorData().laserScanCompressed().isEmpty() &&
(_memory->isIncremental() || lastProximitySpaceClosureId == 0))
{
// In localization mode, no need to check local loop

62
corelib/src/SensorData.cpp
View File

@@ -159,8 +159,7 @@ SensorData::SensorData(
// RGB-D constructor + laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
@@ -170,7 +169,6 @@ SensorData::SensorData(
_id(id),
_stamp(stamp),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(rgb.rows == 1)
@@ -196,13 +194,12 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
if(!laserScan.isCompressed())
{
_laserScanRaw = laserScan;
}
else if(!laserScan.empty())
else
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
@@ -264,8 +261,7 @@ SensorData::SensorData(
// Multi-cameras RGB-D constructor + laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
@@ -275,7 +271,6 @@ SensorData::SensorData(
_id(id),
_stamp(stamp),
_cameraModels(cameraModels),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(rgb.rows == 1)
@@ -301,13 +296,12 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
if(!laserScan.isCompressed())
{
_laserScanRaw = laserScan;
}
else if(!laserScan.empty())
else
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
@@ -371,8 +365,7 @@ SensorData::SensorData(
// Stereo constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
const LaserScanInfo & laserScanInfo,
const LaserScan & laserScan,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
@@ -382,7 +375,6 @@ SensorData::SensorData(
_id(id),
_stamp(stamp),
_stereoCameraModel(cameraModel),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(left.rows == 1)
@@ -407,13 +399,12 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
if(!laserScan.isCompressed())
{
_laserScanRaw = laserScan;
}
else if(!laserScan.empty())
else
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
@@ -565,10 +556,11 @@ void SensorData::setOccupancyGrid(
void SensorData::uncompressData()
{
cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF, tmpG;
cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
LaserScan tmpC;
uncompressData(_imageCompressed.empty()?0:&tmpA,
_depthOrRightCompressed.empty()?0:&tmpB,
_laserScanCompressed.empty()?0:&tmpC,
_laserScanCompressed.isEmpty()?0:&tmpC,
_userDataCompressed.empty()?0:&tmpD,
_groundCellsCompressed.empty()?0:&tmpE,
_obstacleCellsCompressed.empty()?0:&tmpF,
@@ -578,7 +570,7 @@ void SensorData::uncompressData()
void SensorData::uncompressData(
cv::Mat * imageRaw,
cv::Mat * depthRaw,
cv::Mat * laserScanRaw,
LaserScan * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw,
@@ -624,9 +616,13 @@ void SensorData::uncompressData(
{
_depthOrRightRaw = *depthRaw;
}
if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
{
_laserScanRaw = *laserScanRaw;
if(_laserScanCompressed.format() == LaserScan::kUnknown)
{
_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanRaw.format(), _laserScanCompressed.localTransform());
}
}
if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
{
@@ -649,7 +645,7 @@ void SensorData::uncompressData(
void SensorData::uncompressDataConst(
cv::Mat * imageRaw,
cv::Mat * depthRaw,
cv::Mat * laserScanRaw,
LaserScan * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw,
@@ -685,7 +681,7 @@ void SensorData::uncompressDataConst(
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()) ||
(laserScanRaw && laserScanRaw->isEmpty()) ||
(userDataRaw && userDataRaw->empty()) ||
(groundCellsRaw && groundCellsRaw->empty()) ||
(obstacleCellsRaw && obstacleCellsRaw->empty()) ||
@@ -693,7 +689,7 @@ void SensorData::uncompressDataConst(
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed.data(), false);
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
@@ -708,9 +704,9 @@ void SensorData::uncompressDataConst(
UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
ctDepth.start();
}
if(laserScanRaw && laserScanRaw->empty() && !_laserScanCompressed.empty())
if(laserScanRaw && laserScanRaw->isEmpty() && !_laserScanCompressed.isEmpty())
{
UASSERT(_laserScanCompressed.type() == CV_8UC1);
UASSERT(_laserScanCompressed.isCompressed());
ctLaserScan.start();
}
if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
@@ -771,13 +767,13 @@ void SensorData::uncompressDataConst(
}
}
}
if(laserScanRaw && laserScanRaw->empty())
if(laserScanRaw && laserScanRaw->isEmpty())
{
*laserScanRaw = ctLaserScan.getUncompressedData();
*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanCompressed.format(), _laserScanCompressed.localTransform());
if(laserScanRaw->empty())
if(laserScanRaw->isEmpty())
{
if(_laserScanCompressed.empty())
if(_laserScanCompressed.isEmpty())
{
UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
}
@@ -835,8 +831,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
_userDataCompressed.total()*_userDataCompressed.elemSize() +
_userDataRaw.total()*_userDataRaw.elemSize() +
_laserScanCompressed.total()*_laserScanCompressed.elemSize() +
_laserScanRaw.total()*_laserScanRaw.elemSize() +
_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() +
_laserScanRaw.data().total()*_laserScanRaw.data().elemSize() +
_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +

2
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -35,7 +35,7 @@ CREATE TABLE Data (
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_info BLOB, -- scan_max_pts, scan_max_range, local_transform
scan_info BLOB, -- scan_max_pts, scan_max_range, scan_format, local_transform
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)

736
corelib/src/util3d.cpp
View File

@@ -1262,6 +1262,229 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
return scan;
}
LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
{
if(cloud.data.empty())
{
return LaserScan();
}
//determine the output type
int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
pcl::uint32_t fieldOffsets[8] = {0};
for(unsigned int i=0; i<cloud.fields.size(); ++i)
{
if(cloud.fields[i].name.compare("x") == 0)
{
fieldStates[0] = 1;
fieldOffsets[0] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("y") == 0)
{
fieldStates[1] = 1;
fieldOffsets[1] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("z") == 0)
{
fieldStates[2] = 1;
fieldOffsets[2] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("normal_x") == 0)
{
fieldStates[3] = 1;
fieldOffsets[3] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("normal_y") == 0)
{
fieldStates[4] = 1;
fieldOffsets[4] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("normal_z") == 0)
{
fieldStates[5] = 1;
fieldOffsets[5] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("rgb") == 0 || cloud.fields[i].name.compare("rgba") == 0)
{
fieldStates[6] = 1;
fieldOffsets[6] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("intensity") == 0)
{
fieldStates[7] = 1;
fieldOffsets[7] = cloud.fields[i].offset;
}
else
{
UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
}
}
if(fieldStates[0]==0 || fieldStates[1]==0)
{
//should have at least x and y set
UERROR("Cloud has not corresponding fields to laser scan!");
return LaserScan();
}
bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
bool hasIntensity = fieldStates[7];
bool hasRGB = !hasIntensity&&fieldStates[6];
bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
LaserScan::Format format;
if(is3D)
{
if(hasNormals && hasIntensity)
{
format = LaserScan::kXYZINormal;
}
else if(hasNormals && hasRGB)
{
format = LaserScan::kXYZRGBNormal;
}
else if(!hasNormals && hasIntensity)
{
format = LaserScan::kXYZI;
}
else if(!hasNormals && hasRGB)
{
format = LaserScan::kXYZRGB;
}
else
{
format = LaserScan::kXYZ;
}
}
else
{
if(hasNormals && hasIntensity)
{
format = LaserScan::kXYINormal;
}
else if(!hasNormals && hasIntensity)
{
format = LaserScan::kXYI;
}
else
{
format = LaserScan::kXY;
}
}
UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
cv::Mat laserScan(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
int oi=0;
for (uint32_t row = 0; row < cloud.height; ++row)
{
const uint8_t* row_data = &cloud.data[row * cloud.row_step];
for (uint32_t col = 0; col < cloud.width; ++col)
{
const uint8_t* msg_data = row_data + col * cloud.point_step;
float * ptr = laserScan.ptr<float>(0, oi);
if(laserScan.channels() == 2)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
}
else if(laserScan.channels() == 3)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
if(format == LaserScan::kXYI)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZ
{
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
}
}
else if(laserScan.channels() == 4)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
if(format == LaserScan::kXYZI)
{
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZRGB
{
pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
int * ptrInt = (int*)ptr;
ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
}
}
else if(laserScan.channels() == 5)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
}
else if(laserScan.channels() == 6)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
if(format == LaserScan::kXYINormal)
{
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZNormal
{
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
}
ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
}
else if(laserScan.channels() == 7)
{
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
if(format == LaserScan::kXYZINormal)
{
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
}
else // XYZRGBNormal
{
pcl::uint8_t b=*(msg_data + fieldOffsets[6]);
pcl::uint8_t g=*(msg_data + fieldOffsets[6]+1);
pcl::uint8_t r=*(msg_data + fieldOffsets[6]+2);
int * ptrInt = (int*)ptr;
ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
}
ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
}
else
{
UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
}
if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (is3D || uIsFinite(ptr[1])))
{
++oi;
}
}
}
if(laserScan.cols == oi)
{
return LaserScan(laserScan, 0, 0, format);
}
else
{
return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
}
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
@@ -1416,6 +1639,64 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform)
{
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
if(indices.get())
{
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
for(unsigned int i=0; i<indices->size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
int index = indices->at(i);
if(!nullTransform)
{
pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(index).x;
ptr[1] = cloud.at(index).y;
ptr[2] = cloud.at(index).z;
}
ptr[3] = cloud.at(index).intensity;
}
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
}
ptr[3] = cloud.at(i).intensity;
}
}
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
@@ -1488,6 +1769,75 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZINormal pt;
pt.x = cloud.at(i).x;
pt.y = cloud.at(i).y;
pt.z = cloud.at(i).z;
pt.normal_x = normals.at(i).normal_x;
pt.normal_y = normals.at(i).normal_y;
pt.normal_z = normals.at(i).normal_z;
pt = util3d::transformPoint(pt, transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[4] = normals.at(i).normal_x;
ptr[5] = normals.at(i).normal_y;
ptr[6] = normals.at(i).normal_z;
}
ptr[3] = cloud.at(i).intensity;
}
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[4] = pt.normal_x;
ptr[5] = pt.normal_y;
ptr[6] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[4] = cloud.at(i).normal_x;
ptr[5] = cloud.at(i).normal_y;
ptr[6] = cloud.at(i).normal_z;
}
ptr[3] = cloud.at(i).intensity;
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -1512,6 +1862,32 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.intensity;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).intensity;
}
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
@@ -1578,16 +1954,84 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.intensity;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
const pcl::PointXYZINormal & pt = cloud.at(i);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.intensity;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZINormal pt;
pt.x = cloud.at(i).x;
pt.y = cloud.at(i).y;
pt.z = cloud.at(i).z;
pt.normal_x = normals.at(i).normal_x;
pt.normal_y = normals.at(i).normal_y;
pt.normal_z = normals.at(i).normal_z;
pt = util3d::transformPoint(pt, transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.intensity;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).intensity;
ptr[3] = normals.at(i).normal_x;
ptr[4] = normals.at(i).normal_y;
ptr[5] = normals.at(i).normal_z;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = util3d::laserScanToPoint(laserScan, i);
if(!nullTransform)
@@ -1598,15 +2042,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
return output;
}
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
for(int i=0; i<laserScan.cols; ++i)
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = laserScanToPointNormal(laserScan, i);
if(!nullTransform)
@@ -1617,16 +2059,14 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
if(!nullTransform)
@@ -1637,15 +2077,31 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.cols);
pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.cols; ++i)
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = util3d::laserScanToPointI(laserScan, i, intensity);
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
if(!nullTransform)
@@ -1656,72 +2112,77 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
return output;
}
pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
output->at(i) = util3d::laserScanToPointINormal(laserScan, i, intensity);
if(!nullTransform)
{
output->at(i) = util3d::transformPoint(output->at(i), transform);
}
}
return output;
}
pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZ output;
const float * ptr = laserScan.ptr<float>(0, index);
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
if(!laserScan.is2d())
{
output.z = ptr[2];
}
return output;
}
pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
if(!laserScan.is2d())
{
output.z = ptr[2];
}
if(laserScan.channels() == 5)
if(laserScan.hasNormals())
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
}
else if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
output.normal_z = ptr[5];
}
else if(laserScan.channels() == 7)
{
output.normal_x = ptr[4];
output.normal_y = ptr[5];
output.normal_z = ptr[6];
int offset = laserScan.getNormalsOffset();
output.normal_x = ptr[offset];
output.normal_y = ptr[offset+1];
output.normal_z = ptr[offset+2];
}
return output;
}
pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGB output;
const float * ptr = laserScan.ptr<float>(0, index);
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
if(!laserScan.is2d())
{
output.z = ptr[2];
}
if(laserScan.channels() == 4 || laserScan.channels() == 7)
if(laserScan.hasRGB())
{
int * ptrInt = (int*)ptr;
output.b = (unsigned char)(ptrInt[3] & 0xFF);
output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
int indexRGB = laserScan.getRGBOffset();
output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
}
else
{
@@ -1732,42 +2193,50 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
return output;
}
pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZI output;
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
if(!laserScan.is2d())
{
output.z = ptr[2];
}
if(laserScan.channels() == 5)
if(laserScan.hasIntensity())
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
int offset = laserScan.getIntensityOffset();
output.intensity = ptr[offset];
}
else if(laserScan.channels() == 6)
else
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
output.normal_z = ptr[5];
output.intensity = intensity;
}
else if(laserScan.channels() == 7)
return output;
}
pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
{
output.normal_x = ptr[4];
output.normal_y = ptr[5];
output.normal_z = ptr[6];
output.z = ptr[2];
}
if(laserScan.channels() == 4 || laserScan.channels() == 7)
if(laserScan.hasRGB())
{
int * ptrInt = (int*)ptr;
output.b = (unsigned char)(ptrInt[3] & 0xFF);
output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
int indexRGB = laserScan.getRGBOffset();
output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
}
else
{
@@ -1775,6 +2244,48 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int
output.g = g;
output.b = b;
}
if(laserScan.hasNormals())
{
int offset = laserScan.getNormalsOffset();
output.normal_x = ptr[offset];
output.normal_y = ptr[offset+1];
output.normal_z = ptr[offset+2];
}
return output;
}
pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZINormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
{
output.z = ptr[2];
}
if(laserScan.hasIntensity())
{
int offset = laserScan.getIntensityOffset();
output.intensity = ptr[offset];
}
else
{
output.intensity = intensity;
}
if(laserScan.hasNormals())
{
int offset = laserScan.getNormalsOffset();
output.normal_x = ptr[offset];
output.normal_y = ptr[offset+1];
output.normal_z = ptr[offset+2];
}
return output;
}
@@ -2314,66 +2825,59 @@ void savePCDWords(
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
cv::Mat loadBINScan(const std::string & fileName)
{
UASSERT(dim > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cv::Mat output;
long bytes = UFile::length(fileName);
if(bytes)
{
int dim = 4;
UASSERT(bytes % sizeof(float) == 0);
int32_t num = bytes/sizeof(float);
size_t num = bytes/sizeof(float);
UASSERT(num % dim == 0);
float *data = new float[num];
// pointers
float *px = data+0;
float *py = data+1;
float *pz = data+2;
float *pr = data+3;
output = cv::Mat(1, num/dim, CV_32FC(dim));
// load point cloud
FILE *stream;
stream = fopen (fileName.c_str(),"rb");
num = fread(data,sizeof(float),num,stream)/dim;
cloud->resize(num);
for (int32_t i=0; i<num; i++) {
(*cloud)[i].x = *px;
(*cloud)[i].y = *py;
(*cloud)[i].z = *pz;
px+=dim; py+=dim; pz+=dim; pr+=dim;
}
size_t actualReadNum = fread(output.data,sizeof(float),num,stream);
UASSERT(num == actualReadNum);
fclose(stream);
delete[] data;
}
return cloud;
return output;
}
cv::Mat loadScan(
const std::string & path,
const Transform & transform,
int downsampleStep,
float voxelSize,
int normalsK)
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName)
{
cv::Mat scan;
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())
return laserScanToPointCloud(loadScan(fileName));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
return loadBINCloud(fileName);
}
LaserScan loadScan(const std::string & path)
{
std::string fileName = UFile::getName(path);
if(UFile::getExtension(fileName).compare("bin") == 0)
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalsK);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
}
else if(UFile::getExtension(fileName).compare("pcd") == 0)
{
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl::io::loadPCDFile(path, *cloud);
return laserScanFromPointCloud(*cloud);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, transform);
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl::io::loadPLYFile(path, *cloud);
return laserScanFromPointCloud(*cloud);
}
return scan;
return LaserScan();
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(

251
corelib/src/util3d_filtering.cpp
View File

@@ -43,6 +43,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
@@ -65,28 +68,36 @@ namespace rtabmap
namespace util3d
{
cv::Mat rangeFiltering(
const cv::Mat & scan,
void commonFiltering(
LaserScan & scan,
int downsamplingStep,
float rangeMin,
float rangeMax)
float rangeMax,
float voxelSize,
int normalK,
float normalRadius,
bool forceGroundNormalsUp)
{
UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
cv::Mat output;
if(!scan.empty())
UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
if(!scan.isEmpty())
{
if(rangeMin > 0.0f || rangeMax > 0.0f)
// combined downsampling and range filtering step
if(downsamplingStep<=1 || scan.size() <= downsamplingStep)
{
UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3 || scan.type() == CV_32FC(4) || scan.type() == CV_32FC(5) || scan.type() == CV_32FC(6) || scan.type() == CV_32FC(7));
UASSERT(scan.rows == 1);
output = cv::Mat(1, scan.cols, scan.type());
bool is2d = scan.type() == CV_32FC2 || scan.type() == CV_32FC(5);
downsamplingStep = 1;
}
if(downsamplingStep > 1 || rangeMin > 0.0f || rangeMax > 0.0f)
{
cv::Mat tmp = cv::Mat(1, scan.size()/downsamplingStep, scan.dataType());
bool is2d = scan.is2d();
int oi = 0;
float rangeMinSqrd = rangeMin * rangeMin;
float rangeMaxSqrd = rangeMax * rangeMax;
for(int i=0; i<scan.cols; ++i)
for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
{
const float * ptr = scan.ptr<float>(0, i);
const float * ptr = scan.data().ptr<float>(0, i);
float r;
if(is2d)
{
@@ -106,43 +117,216 @@ cv::Mat rangeFiltering(
continue;
}
cv::Mat(scan, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
int previousSize = scan.size();
int scanMaxPtsTmp = scan.maxPoints();
scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, scan.maxRange(), scan.format(), scan.localTransform());
UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
}
else
if(scan.size() && (voxelSize > 0.0f || ((normalK > 0 || normalRadius>0.0f) && !scan.hasNormals())))
{
output = scan.clone();
// convert to compatible PCL format and filter it
if(scan.hasRGB())
{
UASSERT(!scan.is2d());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(scan);
if(cloud->size())
{
int scanMaxPts = scan.maxPoints();
if(voxelSize > 0.0f)
{
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
else
{
if(scan.hasNormals())
{
UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
}
}
}
else if(scan.hasIntensity())
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(scan);
if(cloud->size())
{
int scanMaxPts = scan.maxPoints();
if(voxelSize > 0.0f)
{
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scan.is2d())
{
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
else
{
if(scan.hasNormals())
{
UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYI, scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
}
}
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(scan);
if(cloud->size())
{
int scanMaxPts = scan.maxPoints();
if(voxelSize > 0.0f)
{
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scan.is2d())
{
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
else
{
if(scan.hasNormals())
{
UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXY, scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
}
}
}
}
}
if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
{
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
}
}
}
LaserScan rangeFiltering(
const LaserScan & scan,
float rangeMin,
float rangeMax)
{
UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
if(!scan.isEmpty())
{
if(rangeMin > 0.0f || rangeMax > 0.0f)
{
cv::Mat output = cv::Mat(1, scan.size(), scan.dataType());
bool is2d = scan.is2d();
int oi = 0;
float rangeMinSqrd = rangeMin * rangeMin;
float rangeMaxSqrd = rangeMax * rangeMax;
for(int i=0; i<scan.size(); ++i)
{
const float * ptr = scan.data().ptr<float>(0, i);
float r;
if(is2d)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
}
if(rangeMin > 0.0f && r < rangeMinSqrd)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
}
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
}
}
return output;
return scan;
}
cv::Mat downsample(
const cv::Mat & cloud,
LaserScan downsample(
const LaserScan & scan,
int step)
{
UASSERT(step > 0);
cv::Mat output;
if(step <= 1 || cloud.cols <= step)
if(step <= 1 || scan.size() <= step)
{
// no sampling
output = cloud.clone();
return scan;
}
else
{
int finalSize = cloud.cols/step;
output = cv::Mat(1, finalSize, cloud.type());
int finalSize = scan.size()/step;
cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
int oi = 0;
for(int i=0; i<cloud.cols-step+1; i+=step)
for(int i=0; i<scan.size()-step+1; i+=step)
{
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
return LaserScan(output, scan.maxPoints()/step, scan.maxRange(), scan.format(), scan.localTransform());
}
return output;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
@@ -227,6 +411,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl:
{
return voxelizeImpl<pcl::PointXYZRGBNormal>(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
{
return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
{
@@ -248,6 +436,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(const pcl::PointCloud<pcl:
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, float voxelSize)
{
pcl::IndicesPtr indices(new std::vector<int>);
return voxelize(cloud, indices, voxelSize);
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(

4
corelib/src/util3d_mapping.cpp
View File

@@ -580,8 +580,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
if(jter!=scans.end() && (jter->second.first.cols || jter->second.second.cols))
{
UASSERT(!iter->second.isNull());
cv::Mat hit = util3d::transformLaserScan(jter->second.first, iter->second);
cv::Mat noHit = util3d::transformLaserScan(jter->second.second, iter->second);
cv::Mat hit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.first), iter->second).data();
cv::Mat noHit = util3d::transformLaserScan(LaserScan::backwardCompatibility(jter->second.second), iter->second).data();
pcl::PointXYZ min, max;
if(!hit.empty())
{

302
corelib/src/util3d_surface.cpp
View File

@@ -1206,22 +1206,22 @@ pcl::TextureMesh::Ptr assembleTextureMesh(
if(cloudMat.channels() <= 3)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(cloudMat);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
}
else if(cloudMat.channels() == 4)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(cloudMat);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
}
else if(cloudMat.channels() == 6)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(cloudMat);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
}
else if(cloudMat.channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(cloudMat);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
}
@@ -1322,22 +1322,22 @@ pcl::PolygonMesh::Ptr assemblePolygonMesh(
if(cloudMat.channels() <= 3)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(cloudMat);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
}
else if(cloudMat.channels() == 4)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(cloudMat);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
}
else if(cloudMat.channels() == 6)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(cloudMat);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
}
else if(cloudMat.channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(cloudMat);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
}
@@ -1994,55 +1994,74 @@ cv::Mat mergeTextures(
return globalTextures;
}
cv::Mat computeNormals(
const cv::Mat & laserScan,
LaserScan computeNormals(
const LaserScan & laserScan,
int searchK,
float searchRadius)
{
if(laserScan.empty() || laserScan.channels()<2 || laserScan.channels()>4)
if(laserScan.isEmpty())
{
return laserScan;
}
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(laserScan.channels() < 4)
// convert to compatible PCL format and filter it
if(laserScan.hasRGB())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
if(laserScan.channels() == 2)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(laserScan);
if(cloud->size())
{
normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return util3d::laserScan2dFromPointCloud(*cloud, *normals);
}
else
{
normals = util3d::computeNormals(cloud, searchK, searchRadius);
return util3d::laserScanFromPointCloud(*cloud, *normals);
UASSERT(!laserScan.is2d());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
}
else // 4 channels
else if(laserScan.hasIntensity())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
normals = util3d::computeNormals(cloud, searchK, searchRadius);
return util3d::laserScanFromPointCloud(*cloud, *normals);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(laserScan);
if(cloud->size())
{
if(laserScan.is2d())
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(laserScan);
if(cloud->size())
{
if(laserScan.is2d())
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
else
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
}
}
}
return LaserScan();
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeNormalsImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
@@ -2054,9 +2073,9 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
// Normal estimation*
#ifdef PCL_OMP
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
pcl::NormalEstimationOMP<PointT, pcl::Normal> n;
#else
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::NormalEstimation<PointT, pcl::Normal> n;
#endif
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
@@ -2073,7 +2092,15 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int searchK,
@@ -2083,6 +2110,24 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeNormalsImpl<pcl::PointXYZ>(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -2090,40 +2135,21 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Normal estimation*
#ifdef PCL_OMP
pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
#else
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
#endif
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
n.setInputCloud (cloud);
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// n.setIndices(indices);
//}
n.setSearchMethod (tree);
n.setKSearch (searchK);
n.setRadiusSearch(searchRadius);
n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
n.compute (*normals);
return normals;
return computeNormalsImpl<pcl::PointXYZRGB>(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeNormalsImpl<pcl::PointXYZI>(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2DImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
@@ -2131,7 +2157,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
UASSERT(searchK>0 || searchRadius>0.0f);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud);
normals->resize(cloud->size());
@@ -2141,7 +2167,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
// assuming that points are ordered
for(unsigned int i=0; i<cloud->size(); ++i)
{
const pcl::PointXYZ & pt = cloud->at(i);
const PointT & pt = cloud->at(i);
std::vector<Eigen::Vector3f> neighborNormals;
Eigen::Vector3f direction;
direction[0] = viewPoint[0] - pt.x;
@@ -2163,7 +2189,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
{
if(k_indices.at(j) != (int)i)
{
const pcl::PointXYZ & pt2 = cloud->at(k_indices.at(j));
const PointT & pt2 = cloud->at(k_indices.at(j));
Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
Eigen::Vector3f up = v.cross(direction);
Eigen::Vector3f n = up.cross(v);
@@ -2195,12 +2221,29 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
}
template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2DImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
UASSERT(searchK>0);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
@@ -2225,7 +2268,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
}
// get points before not too far
const pcl::PointXYZ & pt = cloud->at(i);
const PointT & pt = cloud->at(i);
std::vector<Eigen::Vector3f> neighborNormals;
Eigen::Vector3f direction;
direction[0] = viewPoint[0] - cloud->at(i).x;
@@ -2233,7 +2276,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
direction[2] = viewPoint[2] - cloud->at(i).z;
for(int j=i-1; j>=li; --j)
{
const pcl::PointXYZ & pt2 = cloud->at(j);
const PointT & pt2 = cloud->at(j);
Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
{
@@ -2250,7 +2293,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
}
for(int j=i+1; j<=hi; ++j)
{
const pcl::PointXYZ & pt2 = cloud->at(j);
const PointT & pt2 = cloud->at(j);
Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
{
@@ -2290,6 +2333,23 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeFastOrganizedNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
@@ -2339,32 +2399,42 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
}
float computeNormalsComplexity(
const cv::Mat & scan,
const LaserScan & scan,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
if(!scan.isEmpty() && (scan.hasNormals()))
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(scan.cols*2);
bool is2d = scan.channels() == 5;
int sz = static_cast<int>(scan.size()*2);
bool is2d = scan.is2d();
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
for (int i = 0; i < scan.cols; ++i)
int nOffset = 0;
if(!scan.is2d())
{
const float * ptrScan = scan.ptr<float>(0, i);
if(scan.channels() == 5)
nOffset+=1;
}
if(scan.hasIntensity() || scan.hasRGB())
{
nOffset+=1;
}
for (int i = 0; i < scan.size(); ++i)
{
const float * ptrScan = scan.data().ptr<float>(0, i);
if(is2d)
{
if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[2];
ptr[1] = ptrScan[3];
}
}
else if(scan.channels() == 6)
else
{
if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]))
if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]) && uIsFinite(ptrScan[nOffset+4]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[3];
@@ -2372,16 +2442,6 @@ float computeNormalsComplexity(
ptr[2] = ptrScan[5];
}
}
else
{
if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[4];
ptr[1] = ptrScan[5];
ptr[2] = ptrScan[6];
}
}
}
if(oi>1)
{
@@ -2400,7 +2460,7 @@ float computeNormalsComplexity(
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
}
else if(!scan.empty())
else if(!scan.isEmpty())
{
UERROR("Scan doesn't have normals!");
}
@@ -2629,6 +2689,41 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
return cloud_with_normals;
}
LaserScan adjustNormalsToViewPoint(
const LaserScan & scan,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
if(scan.size() && !scan.is2d() && scan.hasNormals())
{
int nx = scan.getNormalsOffset();
int ny = nx+1;
int nz = ny+1;
cv::Mat output = scan.data().clone();
for(unsigned int i=0; i<scan.size(); ++i)
{
float * ptr = output.ptr<float>(0, i);
if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
{
Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
ptr[nx] *= -1.0f;
ptr[ny] *= -1.0f;
ptr[nz] *= -1.0f;
}
}
}
return LaserScan(output, scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
}
return scan;
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
@@ -2644,9 +2739,8 @@ void adjustNormalsToViewPoint(
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//UWARN("Reverse %d of n=%f,%f,%f result=%f", i, cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z, result);
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
@@ -2671,7 +2765,7 @@ void adjustNormalsToViewPoint(
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;

99
corelib/src/util3d_transforms.cpp
View File

@@ -36,52 +36,50 @@ namespace rtabmap
namespace util3d
{
cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
cv::Mat output = laserScan.clone();
cv::Mat output = laserScan.data().clone();
if(!transform.isNull() && !transform.isIdentity())
{
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
for(int i=0; i<laserScan.size(); ++i)
{
const float * ptr = laserScan.ptr<float>(0, i);
const float * ptr = laserScan.data().ptr<float>(0, i);
float * out = output.ptr<float>(0, i);
if(laserScan.type() == CV_32FC2)
if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYI)
{
pcl::PointXYZ pt(ptr[0], ptr[1], 0);
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
}
else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
else if(laserScan.format() == LaserScan::kXYZ || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZRGB)
{
const float * ptr = laserScan.ptr<float>(0, i);
pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]);
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
}
else if(laserScan.type() == CV_32FC(5))
else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYINormal)
{
int nOffset = laserScan.format() == LaserScan::kXYINormal?1:0;
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=0;
pt.normal_x=ptr[2];
pt.normal_y=ptr[3];
pt.normal_z=ptr[4];
pt.normal_x=ptr[nOffset+2];
pt.normal_y=ptr[nOffset+3];
pt.normal_z=ptr[nOffset+4];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.normal_x;
out[3] = pt.normal_y;
out[4] = pt.normal_z;
out[nOffset+2] = pt.normal_x;
out[nOffset+3] = pt.normal_y;
out[nOffset+4] = pt.normal_z;
}
else if(laserScan.type() == CV_32FC(6))
else if(laserScan.format() == LaserScan::kXYZNormal)
{
pcl::PointNormal pt;
pt.x=ptr[0];
@@ -98,7 +96,7 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
out[4] = pt.normal_y;
out[5] = pt.normal_z;
}
else // 7 channels
else if(laserScan.format() == LaserScan::kXYZINormal || laserScan.format() == LaserScan::kXYZRGBNormal)
{
pcl::PointNormal pt;
pt.x=ptr[0];
@@ -115,9 +113,13 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
out[5] = pt.normal_y;
out[6] = pt.normal_z;
}
else
{
UERROR("Unknown laser scan format! (%d)", laserScan.format());
}
}
}
return output;
return LaserScan(output, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
@@ -128,6 +130,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZI>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform)
@@ -152,6 +162,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -162,6 +180,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZI>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -189,6 +216,15 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
return output;
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr transformPointCloud(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
return output;
}
cv::Point3f transformPoint(
const cv::Point3f & point,
@@ -206,6 +242,12 @@ pcl::PointXYZ transformPoint(
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
pcl::PointXYZI transformPoint(
const pcl::PointXYZI & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
pcl::PointXYZRGB transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform)
@@ -250,6 +292,25 @@ pcl::PointXYZRGBNormal transformPoint(
ret.rgb = point.rgb;
return ret;
}
pcl::PointXYZINormal transformPoint(
const pcl::PointXYZINormal & point,
const Transform & transform)
{
pcl::PointXYZINormal ret;
Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
// Rotate normals
Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
ret.intensity = point.intensity;
return ret;
}
}