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

0.11.10: Database update with occupancy grid and laser scan info. Added class LaserScanInfo and OccupancyGrid (incremental 2d grid map). Gui: 2d grid and octomap are udpated using occupancy grids saved in nodes.

Browse Source
This commit is contained in:
matlabbe
2016-08-21 19:33:01 -04:00
parent af02e02978
commit 013eba1d58
49 changed files with 3376 additions and 1259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -115,6 +115,7 @@ public:
bool save(const std::string & directory) const;
CameraModel scaled(double scale) const;
CameraModel roi(const cv::Rect & roi) const;
double horizontalFOV() const; // in degrees
double verticalFOV() const; // in degrees

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

@@ -118,8 +118,8 @@ public:
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures) const;
void getNodeData(int signatureId, SensorData & data) const;
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 getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -171,7 +171,7 @@ private:
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;

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

@@ -0,0 +1,65 @@
/*
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_ */

6
corelib/include/rtabmap/core/Memory.h
View File

@@ -55,7 +55,7 @@ class Registration;
class RegistrationInfo;
class RegistrationIcp;
class Stereo;
class Occupancy;
class OccupancyGrid;
class RTABMAP_EXP Memory
{
@@ -156,7 +156,7 @@ public:
void getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel);
SensorData getSignatureDataConst(int locationId) const;
SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
@@ -277,7 +277,7 @@ private:
Registration * _registrationPipeline;
RegistrationIcp * _registrationIcp;
Occupancy * _occupancy;
OccupancyGrid * _occupancy;
};
} // namespace rtabmap

40
corelib/include/rtabmap/core/Occupancy.h → corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_SRC_OCCUPANCY_H_
#define CORELIB_SRC_OCCUPANCY_H_
#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
#define CORELIB_SRC_OCCUPANCYGRID_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
@@ -35,34 +35,62 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class RTABMAP_EXP Occupancy
class RTABMAP_EXP OccupancyGrid
{
public:
Occupancy(const ParametersMap & parameters = ParametersMap());
OccupancyGrid(const ParametersMap & parameters = ParametersMap());
void parseParameters(const ParametersMap & parameters);
void setCellSize(float cellSize);
float getCellSize() const {return cellSize_;}
void segment(const Signature & node, cv::Mat & obstacles, cv::Mat & ground);
void createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const;
void clear();
void addToCache(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles);
void update(const std::map<int, Transform> & poses, float minMapSize = 0.0f, float footprintRadius = 0.0f);
const cv::Mat & getMap(float & xMin, float & yMin) const
{
xMin = xMin_;
yMin = yMin_;
return map_;
}
private:
ParametersMap parameters_;
int cloudDecimation_;
float cloudMaxDepth_;
float cloudMinDepth_;
std::vector<float> roiRatios_;
int scanDecimation_;
float cellSize_;
bool occupancyFromCloud_;
bool projMapFrame_;
float maxObstacleHeight_;
int normalKSearch_;
float maxGroundAngle_;
int minClusterSize_;
bool flatObstaclesDetected_;
float minGroundHeight_;
float maxGroundHeight_;
bool normalsSegmentation_;
bool grid3D_;
bool groundIsObstacle_;
float noiseFilteringRadius_;
int noiseFilteringMinNeighbors_;
bool scan2dUnknownSpaceFilled_;
double scan2dMaxUnknownSpaceFilledRange_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
cv::Mat map_;
cv::Mat mapInfo_;
std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
float xMin_;
float yMin_;
std::map<int, Transform> addedNodes_;
};
}
#endif /* CORELIB_SRC_OCCUPANCY_H_ */
#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */

12
corelib/include/rtabmap/core/OctoMap.h
View File

@@ -62,7 +62,12 @@ public:
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
const pcl::PointXYZ & viewPoint);
void addToCache(int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Point3f & viewPoint);
void update(const std::map<int, Transform> & poses);
const octomap::ColorOcTree * octree() const {return octree_;}
@@ -84,11 +89,14 @@ public:
void clear();
private:
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
std::map<int, cv::Point3f> cacheViewPoints_;
octomap::ColorOcTree * octree_;
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
std::map<int, Transform> addedNodes_;
octomap::KeyRay keyRay_;
bool hasColor_;
};
} /* namespace rtabmap */

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

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// default parameters
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Version.h" // DLL export/import defines
#include <rtabmap/utilite/UConversion.h>
#include <string>
#include <map>
@@ -176,7 +177,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
RTABMAP_PARAM(Rtabmap, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes, bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, "", "Working directory.");
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
@@ -210,7 +211,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, false, "Use odometry features.");
RTABMAP_PARAM(Mem, CreateOccupancyGrid, bool, true, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -308,7 +308,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1, uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
RTABMAP_PARAM(RGBD, PlanLinearVelocity, float, 0, "Linear velocity (m/sec) used to compute path weights.");
@@ -320,6 +320,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data.");
RTABMAP_PARAM(RGBD, NeighborLinkRefining, bool, false, "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes.");
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
@@ -344,7 +345,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Optimizer, Slam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"RGBD/OptimizeMaxError\" if enabled.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod");
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
@@ -391,12 +392,12 @@ class RTABMAP_EXP Parameters
// Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(Vis, RefineIterations, int, 5, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, float, 2, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, "[Vis/EstimationType = 1] Refine iterations.");
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPFlags, int, 1, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
#ifndef RTABMAP_NONFREE
@@ -418,13 +419,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Vis, SubPixEps, float, 0.02, "See cv::cornerSubPix().");
RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
RTABMAP_PARAM(Vis, CorNNType, int, 1, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorGuessWinSize, int, 50, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorGuessWinSize, int, 50, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
// ICP registration parameters
RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
@@ -446,8 +447,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Stereo, MinDisparity, int, 1, "Minimum disparity.");
RTABMAP_PARAM(Stereo, MaxDisparity, int, 128, "Maximum disparity.");
RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
RTABMAP_PARAM(Stereo, SSD, bool, true, "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
RTABMAP_PARAM(Stereo, Eps, double, 0.01, "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
RTABMAP_PARAM(Stereo, SSD, bool, true, uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
RTABMAP_PARAM(Stereo, Eps, double, 0.01, uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
RTABMAP_PARAM(StereoBM, BlockSize, int, 15, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, MinDisparity, int, 0, "See cv::StereoBM");
@@ -460,22 +461,32 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(StereoBM, SpeckleRange, int, 4, "See cv::StereoBM");
// Occupancy Grid
RTABMAP_PARAM(Grid, FromDepth, bool, false, "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
RTABMAP_PARAM(Grid, DepthDecimation, int, 1, "[Grid/FromDepth=true]");
RTABMAP_PARAM(Grid, DepthMin, float, 0.0, "[Grid/FromDepth=true]");
RTABMAP_PARAM(Grid, DepthMax, float, 0.0, "[Grid/FromDepth=true]");
RTABMAP_PARAM_STR(Grid, DepthRoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Grid, CellSize, float, 0.05, "");
RTABMAP_PARAM(Grid, MapFrameProjection, bool, false, "");
RTABMAP_PARAM(Grid, MaxObstacleHeight, float, 0.0, "");
RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, "");
RTABMAP_PARAM(Grid, MaxGroundAngle, float, 0.78, "");
RTABMAP_PARAM(Grid, MinClusterSize, int, 10, "");
RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, false, "");
RTABMAP_PARAM(Grid, 3D, bool, false, "Ignored if laser scan is 2D.");
RTABMAP_PARAM(Grid, 3DGroundIsObstacle, bool, false, "[Grid/3D=true] The ground is considered as an obstacle.");
RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "0 means disabled.");
RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "");
RTABMAP_PARAM(Grid, FromDepth, bool, true, "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
RTABMAP_PARAM(Grid, DepthDecimation, int, 4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, DepthMin, float, 0.0, uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, DepthMax, float, 4.0, uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM_STR(Grid, DepthRoiRatios, "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, ScanDecimation, int, 1, uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridDepthDecimation().c_str()));
RTABMAP_PARAM(Grid, CellSize, float, 0.05, "Resolution of the occupancy grid.");
RTABMAP_PARAM(Grid, MapFrameProjection, bool, false, "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
RTABMAP_PARAM(Grid, NormalsSegmentation, bool, true, "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
RTABMAP_PARAM(Grid, MaxObstacleHeight, float, 0.0, "Maximum obstacles height (0=disabled).");
RTABMAP_PARAM(Grid, MinGroundHeight, float, 0.0, "Minimum ground height (0=disabled).");
RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, MaxGroundAngle, float, 45, uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NormalK, int, 10, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
RTABMAP_PARAM(Grid, MinClusterSize, int, 10, uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, false, uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
#ifdef RTABMAP_OCTOMAP
RTABMAP_PARAM(Grid, 3D, bool, true, uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
#else
RTABMAP_PARAM(Grid, 3D, bool, false, uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
#endif
RTABMAP_PARAM(Grid, 3DGroundIsObstacle, bool, false, uFormat("[%s=true] Ground is an obstacle. Use this only if you want an Octomap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str()));
RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "Noise filtering radius (0=disabled). Done after segmentation.");
RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors.");
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, "Unknown space filled. Only used with 2D laser scans.");
RTABMAP_PARAM(Grid, Scan2dMaxFilledRange, float, 6.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
public:
virtual ~Parameters();
@@ -501,12 +512,12 @@ public:
*/
static std::string getDescription(const std::string & paramKey);
static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
static void parse(const ParametersMap & parameters, const std::string & key, int & value);
static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
static void parse(const ParametersMap & parameters, const std::string & key, float & value);
static void parse(const ParametersMap & parameters, const std::string & key, double & value);
static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
static bool parse(const ParametersMap & parameters, const std::string & key, bool & value);
static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
static bool parse(const ParametersMap & parameters, const std::string & key, double & value);
static bool parse(const ParametersMap & parameters, const std::string & key, std::string & value);
static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
static const char * showUsage();

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

@@ -33,6 +33,7 @@ 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 <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -75,8 +76,7 @@ public:
// RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
@@ -96,8 +96,7 @@ public:
// Multi-cameras RGB-D constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
@@ -117,8 +116,7 @@ public:
// Stereo constructor + laser scan
SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
@@ -131,7 +129,6 @@ public:
bool isValid() const {
return !(_id == 0 &&
_stamp == 0.0 &&
_laserScanMaxPts == 0 &&
_imageRaw.empty() &&
_imageCompressed.empty() &&
_depthOrRightRaw.empty() &&
@@ -150,8 +147,7 @@ public:
void setId(int id) {_id = id;}
double stamp() const {return _stamp;}
void setStamp(double stamp) {_stamp = stamp;}
int laserScanMaxPts() const {return _laserScanMaxPts;}
float laserScanMaxRange() const {return _laserScanMaxRange;}
const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
const cv::Mat & imageCompressed() const {return _imageCompressed;}
const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
@@ -162,7 +158,7 @@ public:
const cv::Mat & 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, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
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,8 +168,20 @@ public:
cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
void uncompressData();
void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0);
void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
void uncompressData(
cv::Mat * imageRaw,
cv::Mat * depthOrRightRaw,
cv::Mat * laserScanRaw = 0,
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0);
void uncompressDataConst(
cv::Mat * imageRaw,
cv::Mat * depthOrRightRaw,
cv::Mat * laserScanRaw = 0,
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0) const;
const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
@@ -183,6 +191,21 @@ public:
const cv::Mat & userDataRaw() const {return _userDataRaw;}
const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
// detect automatically if raw or compressed. If raw, the data will be compressed.
void setOccupancyGrid(
const cv::Mat & ground,
const cv::Mat & obstacles,
float cellSize,
const cv::Point3f & viewPoint);
// remove raw occupancy grids
void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
float gridCellSize() const {return _cellSize;}
const cv::Point3f & gridViewPoint() const {return _viewPoint;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
_keypoints = keypoints;
@@ -199,8 +222,6 @@ public:
private:
int _id;
double _stamp;
int _laserScanMaxPts;
float _laserScanMaxRange;
cv::Mat _imageCompressed; // compressed image
cv::Mat _depthOrRightCompressed; // compressed image
@@ -213,10 +234,20 @@ private:
std::vector<CameraModel> _cameraModels;
StereoCameraModel _stereoCameraModel;
LaserScanInfo _laserScanInfo;
// user data
cv::Mat _userDataCompressed; // compressed data
cv::Mat _userDataRaw;
// occupancy grid
cv::Mat _groundCellsCompressed;
cv::Mat _obstacleCellsCompressed;
cv::Mat _groundCellsRaw;
cv::Mat _obstacleCellsRaw;
float _cellSize;
cv::Point3f _viewPoint;
// features
std::vector<cv::KeyPoint> _keypoints;
cv::Mat _descriptors;

15
corelib/include/rtabmap/core/Signature.h
View File

@@ -115,22 +115,11 @@ public:
void setPose(const Transform & pose) {_pose = pose;}
void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
void setOccupancyGrid(const cv::Mat & ground, const cv::Mat & obstacles, float cellSize)
{
_groundCells = ground.clone();
_obstacleCells = obstacles.clone();
_cellSize = cellSize;
}
const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
const Transform & getPose() const {return _pose;}
cv::Mat getPoseCovariance() const;
const Transform & getGroundTruthPose() const {return _groundTruthPose;}
const cv::Mat & getGroundCells() const {return _groundCells;}
const cv::Mat & getObstacleCells() const {return _obstacleCells;}
const float getCellSize() const {return _cellSize;}
SensorData & sensorData() {return _sensorData;}
const SensorData & sensorData() const {return _sensorData;}
@@ -157,10 +146,6 @@ private:
Transform _pose;
Transform _groundTruthPose;
cv::Mat _groundCells;
cv::Mat _obstacleCells;
float _cellSize;
SensorData _sensorData;
};

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

@@ -108,6 +108,7 @@ public:
const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
void scale(double scale);
void roi(const cv::Rect & roi);
void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
const Transform & localTransform() const {return left_.localTransform();}

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

@@ -242,7 +242,7 @@ void occupancy2DFromGroundObstacles(
//voxelize to grid cell size
groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
{
ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
@@ -259,7 +259,7 @@ void occupancy2DFromGroundObstacles(
//voxelize to grid cell size
obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;

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

@@ -109,6 +109,11 @@ float RTABMAP_EXP getDepth(
float maxZError = 0.02f,
bool estWithNeighborsIfNull = false);
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios);
cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::string & roiRatios);
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios);
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f);

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

@@ -141,7 +141,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap());
const ParametersMap & parameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
* Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -154,6 +155,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
* @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
* @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
* @param validIndices, the indices of valid points in the cloud
* @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
* @return a RGB cloud.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
@@ -162,7 +164,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & parameters = ParametersMap());
const ParametersMap & parameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
const cv::Mat & depthImage,
@@ -178,12 +181,24 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC4
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
// return CV_32FC2
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, 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 or CV_32FC3, normals are set to null.
// 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 null.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, 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 null.
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index);
cv::Point3f RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,

2
corelib/src/CMakeLists.txt
View File

@@ -65,7 +65,7 @@ SET(SRC_FILES
StereoDense.cpp
StereoCameraModel.cpp
Occupancy.cpp
OccupancyGrid.cpp
rtflann/ext/lz4.c
rtflann/ext/lz4hc.c

30
corelib/src/CameraModel.cpp
View File

@@ -383,6 +383,36 @@ CameraModel CameraModel::scaled(double scale) const
return scaledModel;
}
CameraModel CameraModel::roi(const cv::Rect & roi) const
{
CameraModel roiModel = *this;
if(this->isValidForProjection())
{
// has only effect on cx and cy
cv::Mat K;
if(!K_.empty())
{
K = K_.clone();
K.at<double>(0,2) -= roi.x;
K.at<double>(1,2) -= roi.y;
}
cv::Mat P;
if(!P_.empty())
{
P = P_.clone();
P.at<double>(0,2) -= roi.x;
P.at<double>(1,2) -= roi.y;
}
roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
}
else
{
UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
}
return roiModel;
}
double CameraModel::horizontalFOV() const
{
if(imageWidth() > 0 && fx() > 0.0)

7
corelib/src/CameraRGB.cpp
View File

@@ -65,6 +65,7 @@ CameraImages::CameraImages() :
_dir(0),
_countScan(0),
_scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0),
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
@@ -666,11 +667,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform.inverse());
}
}
}
@@ -684,7 +685,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
_model.setImageSize(img.size());
}
SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
return data;
}

2
corelib/src/CameraStereo.cpp
View File

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

29
corelib/src/CameraThread.cpp
View File

@@ -175,9 +175,15 @@ void CameraThread::mainLoop()
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
_scanMinDepth,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
if(validIndices->size())
{
if(_scanVoxelSize>0.0f)
@@ -197,32 +203,19 @@ void CameraThread::mainLoop()
{
if(_scanNormalsK>0)
{
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
}
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
}
}
}
data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
info.timeScanFromDepth = timer.ticks();
UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
}

15
corelib/src/DBDriver.cpp
View File

@@ -514,7 +514,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
}
}
void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
{
// Don't look in the trash, we assume that if we want to load
// data of a signature, it is not in thrash! Print an error if so.
@@ -530,13 +530,14 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
_trashesMutex.unlock();
_dbSafeAccessMutex.lock();
this->loadNodeDataQuery(signatures);
this->loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
_dbSafeAccessMutex.unlock();
}
void DBDriver::getNodeData(
int signatureId,
SensorData & data) const
SensorData & data,
bool images, bool scan, bool userData, bool occupancyGrid) const
{
bool found = false;
// look in the trash
@@ -544,7 +545,11 @@ void DBDriver::getNodeData(
if(uContains(_trashSignatures, signatureId))
{
const Signature * s = _trashSignatures.at(signatureId);
if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
if(!s->sensorData().imageCompressed().empty() ||
!s->sensorData().laserScanCompressed().empty() ||
!s->sensorData().userDataCompressed().empty() ||
s->sensorData().gridCellSize() != 0.0f ||
!s->isSaved())
{
data = (SensorData)s->sensorData();
found = true;
@@ -558,7 +563,7 @@ void DBDriver::getNodeData(
std::list<Signature *> signatures;
Signature tmp(signatureId);
signatures.push_back(&tmp);
loadNodeDataQuery(signatures);
loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
data = signatures.front()->sensorData();
_dbSafeAccessMutex.unlock();
}

504
corelib/src/DBDriverSqlite3.cpp
View File

@@ -744,9 +744,16 @@ ParametersMap DBDriverSqlite3::getLastParametersQuery() const
return parameters;
}
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("load data for %d signatures", (int)signatures.size());
if(!images && !scan && !userData && !occupancyGrid)
{
UWARN("All requested data fields are false! Nothing loaded...");
return;
}
if(_ppDb)
{
UTimer timer;
@@ -755,7 +762,44 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.10.7") >= 0)
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
std::stringstream fields;
if(images)
{
fields << "image, depth, calibration";
if(scan || userData || occupancyGrid)
{
fields << ", ";
}
}
if(scan)
{
fields << "scan_info, scan";
if(userData || occupancyGrid)
{
fields << ", ";
}
}
if(userData)
{
fields << "user_data";
if(occupancyGrid)
{
fields << ", ";
}
}
if(occupancyGrid)
{
fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
}
query << "SELECT " << fields.str().c_str() << " "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
<< "FROM Data "
@@ -853,197 +897,259 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
cv::Mat scanCompressed;
cv::Mat userDataCompressed;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the image
if(dataSize>4 && data)
{
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
if(dataSize>4 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
if(uStrNumCmp(_version, "0.11.10") < 0 || images)
{
//Create the image
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
if(dataSize>4 && data)
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
//Create the depth image
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(6+localTransform.size());
for(int i=0; i<max; i+=6+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
else if(uStrNumCmp(_version, "0.11.2") < 0 &&
(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
}
else
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
float laserScanMaxRange = 0.0f;
if(uStrNumCmp(_version, "0.10.7") >= 0)
Transform scanLocalTransform = Transform::getIdentity();
if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
{
laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
}
// scan_info
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
if(dataSize>4 && data)
{
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
laserScanMaxPts = (int)dataFloat[0];
laserScanMaxRange = dataFloat[1];
}
}
else
{
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
if(uStrNumCmp(_version, "0.10.7") >= 0)
{
laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
}
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
//Create the laserScan
if(dataSize>4 && data)
{
if(uStrNumCmp(_version, "0.10.1") >= 0)
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
}
if(uStrNumCmp(_version, "0.11.10") < 0 || userData)
{
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the userData
if(dataSize>4 && data)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
else
{
// compress data (set uncompressed data to signed to make difference with compressed type)
userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
else
{
// compress data (set uncompressed data to signed to make difference with compressed type)
userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
}
}
}
}
// Occupancy grid
cv::Mat groundCellsCompressed;
cv::Mat obstacleCellsCompressed;
float cellSize = 0.0f;
cv::Point3f viewPoint;
if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
{
// ground
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
groundCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)groundCellsCompressed.data, data, dataSize);
}
// obstacle
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
obstacleCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
}
cellSize = sqlite3_column_double(ppStmt, index++);
viewPoint.x = sqlite3_column_double(ppStmt, index++);
viewPoint.y = sqlite3_column_double(ppStmt, index++);
viewPoint.z = sqlite3_column_double(ppStmt, index++);
}
SensorData tmp = (*iter)->sensorData();
if(models.size())
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
laserScanMaxRange,
imageCompressed,
depthOrRightCompressed,
models,
scan?scanCompressed:tmp.laserScanCompressed(),
scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
images?imageCompressed:tmp.imageCompressed(),
images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
images?models:tmp.cameraModels(),
(*iter)->id(),
0,
userDataCompressed);
(*iter)->getStamp(),
userData?userDataCompressed:tmp.userDataCompressed());
}
else
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
laserScanMaxRange,
imageCompressed,
depthOrRightCompressed,
stereoModel,
scan?scanCompressed:tmp.laserScanCompressed(),
scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
images?imageCompressed:tmp.imageCompressed(),
images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
images?stereoModel:tmp.stereoCameraModel(),
(*iter)->id(),
0,
userDataCompressed);
(*iter)->getStamp(),
userData?userDataCompressed:tmp.userDataCompressed());
}
if(occupancyGrid)
{
(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, cellSize, viewPoint);
}
else
{
(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
}
rc = sqlite3_step(ppStmt); // next result...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1726,7 +1832,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if(uStrNumCmp(_version, "0.11.1") >= 0)
{
data = sqlite3_column_blob(ppStmt, index); // pose
data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
@@ -1901,7 +2007,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
int calibrationsLoaded = 0;
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
// bind id
@@ -1925,7 +2030,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
++calibrationsLoaded;
float * dataFloat = (float*)data;
if(uStrNumCmp(_version, "0.11.2") >= 0 &&
(unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
@@ -2003,8 +2107,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
}
if(ids.size() != loaded)
if(ids.size() != loaded)
{
UERROR("Some signatures not found in database");
}
@@ -3017,6 +3120,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3163,7 +3267,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().maxPoints());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
@@ -3178,7 +3282,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.10.7") >= 0)
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
}
@@ -3293,15 +3401,43 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
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)
std::vector<float> scanInfo;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange());
if(sensorData.laserScanInfo().maxPoints() > 0 ||
sensorData.laserScanInfo().maxRange() > 0 ||
(!sensorData.laserScanInfo().localTransform().isNull() && !sensorData.laserScanInfo().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(scanInfo.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// scan_max_pts
rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().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());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// scan
@@ -3329,6 +3465,46 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
//ground_cells
if(sensorData.gridGroundCellsCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
// compress
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//obstacle_cells
if(sensorData.gridObstacleCellsCompressed().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//cell_size
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//view_point
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -83,7 +83,7 @@ private:
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
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 getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;

70
corelib/src/Features2d.cpp
View File

@@ -261,78 +261,12 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
{
std::list<std::string> strValues = uSplit(roiRatios, ' ');
if(strValues.size() != 4)
{
UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
}
else
{
std::vector<float> values(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
values[i] = uStr2Float(*iter);
++i;
}
if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
{
return computeRoi(image, values);
}
else
{
UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
}
}
return cv::Rect();
return util2d::computeRoi(image, roiRatios);
}
cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
if(!image.empty() && roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
{
roi.x = width * roiRatios[0];
}
//right roi
if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
{
roi.width -= width * roiRatios[1] + width * roiRatios[0];
}
//top roi
if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
{
roi.y = height * roiRatios[2];
}
//bottom roi
if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
{
roi.height -= height * roiRatios[3] + height * roiRatios[2];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
return cv::Rect();
}
return util2d::computeRoi(image, roiRatios);
}
/////////////////////

101
corelib/src/Memory.cpp
View File

@@ -57,10 +57,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/core/Occupancy.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <rtabmap/core/OccupancyGrid.h>
namespace rtabmap {
@@ -92,7 +91,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_createOccupancyGrid(Parameters::defaultMemCreateOccupancyGrid()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -109,7 +108,7 @@ Memory::Memory(const ParametersMap & parameters) :
_vwd = new VWDictionary(parameters);
_registrationPipeline = Registration::create(parameters);
_registrationIcp = new RegistrationIcp(parameters);
_occupancy = new Occupancy(parameters);
_occupancy = new OccupancyGrid(parameters);
this->parseParameters(parameters);
}
@@ -413,7 +412,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(parameters, Parameters::kMemCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -2062,7 +2061,7 @@ void Memory::removeRawData(int id, bool image, bool scan, bool userData)
}
if(scan && !_registrationPipeline->isScanRequired())
{
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanInfo());
}
if(userData && !_registrationPipeline->isUserDataRequired())
{
@@ -2305,7 +2304,9 @@ Transform Memory::computeIcpTransformMulti(
{
cv::Mat scan;
s->sensorData().uncompressData(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
{
maxPoints = scan.cols;
@@ -2320,7 +2321,12 @@ Transform Memory::computeIcpTransformMulti(
}
if(assembledToClouds->size())
{
assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
assembledData.setLaserScanRaw(
util3d::laserScanFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
@@ -2985,52 +2991,23 @@ void Memory::getNodeCalibration(int nodeId,
}
}
SensorData Memory::getSignatureDataConst(int locationId) const
SensorData Memory::getSignatureDataConst(int locationId,
bool images, bool scan, bool userData, bool occupancyGrid) const
{
UDEBUG("");
SensorData r;
const Signature * s = this->getSignature(locationId);
if(s && !s->sensorData().imageCompressed().empty())
if(s && (!s->sensorData().imageCompressed().empty() ||
!s->sensorData().laserScanCompressed().empty() ||
!s->sensorData().userDataCompressed().empty() ||
s->sensorData().gridCellSize() != 0.0f))
{
r = s->sensorData();
}
else if(_dbDriver)
{
// load from database
if(s)
{
std::list<Signature*> signatures;
Signature tmp = *s;
signatures.push_back(&tmp);
_dbDriver->loadNodeData(signatures);
r = tmp.sensorData();
}
else
{
std::list<int> ids;
ids.push_back(locationId);
std::list<Signature*> signatures;
std::set<int> loadedFromTrash;
_dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
if(signatures.size())
{
Signature * sTmp = signatures.front();
if(sTmp->sensorData().imageCompressed().empty())
{
_dbDriver->loadNodeData(signatures);
}
r = sTmp->sensorData();
if(loadedFromTrash.size())
{
//put it back to trash
_dbDriver->asyncSave(sTmp);
}
else
{
delete sTmp;
}
}
}
_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
}
return r;
@@ -3502,7 +3479,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
// downsampling the laser scan?
cv::Mat laserScan = data.laserScanRaw();
int maxLaserScanMaxPts = data.laserScanMaxPts();
int maxLaserScanMaxPts = data.laserScanInfo().maxPoints();
if(!laserScan.empty() && _laserScanDownsampleStepSize > 1)
{
laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
@@ -3554,8 +3531,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
stereoCameraModel.isValidForProjection()?
SensorData(
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
stereoCameraModel,
@@ -3564,8 +3540,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
ctUserData.getCompressedData()):
SensorData(
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
cameraModels,
@@ -3575,12 +3550,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
else
{
// just compress laser and user data
rtabmap::CompressionThread ctLaserScan(laserScan);
// just compress user data
rtabmap::CompressionThread ctUserData(data.userDataRaw());
ctLaserScan.start();
ctUserData.start();
ctLaserScan.join();
ctUserData.join();
s = new Signature(id,
@@ -3592,9 +3564,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
SensorData(
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
cv::Mat(),
LaserScanInfo(),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
@@ -3602,9 +3573,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
0,
ctUserData.getCompressedData()):
SensorData(
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
cv::Mat(),
LaserScanInfo(),
cv::Mat(),
cv::Mat(),
cameraModels,
@@ -3620,7 +3590,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, maxLaserScanMaxPts, data.laserScanMaxRange());
s->sensorData().setLaserScanRaw(laserScan, LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()));
s->sensorData().setUserDataRaw(data.userDataRaw());
s->sensorData().setGroundTruth(data.groundTruth());
@@ -3634,19 +3604,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
// Occupancy grid map stuff
/*cv::Mat ground, obstacles;
cv::Mat ground, obstacles;
float cellSize = 0.0f;
cv::Point3f viewPoint(0,0,0);
if(_createOccupancyGrid)
{
_occupancy->segment(s->sensorData(), ground, obstacles);
_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
cellSize = _occupancy->getCellSize();
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
UDEBUG("time grid map (%d) = %fs", t);
UDEBUG("time grid map = %fs", t);
}
s->setOccupancyGrid(ground, obstacles, cellSize);
*/
s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
return s;
}

201
corelib/src/Occupancy.cpp
View File

@@ -1,201 +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.
*/
#include <rtabmap/core/Occupancy.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
Occupancy::Occupancy(const ParametersMap & parameters) :
parameters_(parameters),
cloudDecimation_(Parameters::defaultGridDepthDecimation()),
cloudMaxDepth_(Parameters::defaultGridDepthMax()),
cloudMinDepth_(Parameters::defaultGridDepthMin()),
cellSize_(Parameters::defaultGridCellSize()),
occupancyFromCloud_(Parameters::defaultGridFromDepth()),
projMapFrame_(Parameters::defaultGridMapFrameProjection()),
maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()),
minClusterSize_(Parameters::defaultGridMinClusterSize()),
flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
grid3D_(Parameters::defaultGrid3D()),
groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors())
{
this->parseParameters(parameters);
}
void Occupancy::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_);
Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
}
void Occupancy::segment(const Signature & node, cv::Mat & obstacles, cv::Mat & ground)
{
if(!occupancyFromCloud_ && node.sensorData().laserScanRaw().channels() == 2)
{
//2D
util3d::occupancy2DFromLaserScan(
node.sensorData().laserScanRaw(),
ground,
obstacles,
cellSize_);
}
else
{
// 3D
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(!occupancyFromCloud_)
{
cloud =util3d::laserScanToPointCloud(node.sensorData().laserScanRaw());
}
else
{
cloud = util3d::cloudFromSensorData(
node.sensorData(),
cloudDecimation_,
cloudMaxDepth_,
cloudMinDepth_,
indices.get(),
parameters_);
}
if(cloud->size())
{
// voxelize to grid cell size
cloud = util3d::voxelize(cloud, indices, cellSize_);
indices->clear();
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 &&
noiseFilteringMinNeighbors_ > 0)
{
indices = rtabmap::util3d::radiusFiltering(
cloud,
noiseFilteringRadius_,
noiseFilteringMinNeighbors_);
if(indices->empty())
{
UWARN("Cloud (with %d points) is empty after noise "
"filtering. Occupancy grid of node %d cannot be "
"created.",
(int)cloud->size(), node.id());
return;
}
}
// add pose rotation without yaw
float roll, pitch, yaw;
node.getPose().getEulerAngles(roll, pitch, yaw);
if(indices->size())
{
cloud = util3d::transformPointCloud(cloud, indices, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
}
else
{
cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
}
if(maxObstacleHeight_ != 0.0f)
{
cloud = util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstacleHeight_);
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
cloud,
groundIndices,
obstaclesIndices,
20,
maxGroundAngle_,
cellSize_*2.0f,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
}
if(grid3D_)
{
if(groundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
// transform back in base frame
Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
}
else
{
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
cellSize_);
}
}
}
}
}

979
corelib/src/OccupancyGrid.cpp Normal file
View File

@@ -0,0 +1,979 @@
/*
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/OccupancyGrid.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
namespace rtabmap {
OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
parameters_(parameters),
cloudDecimation_(Parameters::defaultGridDepthDecimation()),
cloudMaxDepth_(Parameters::defaultGridDepthMax()),
cloudMinDepth_(Parameters::defaultGridDepthMin()),
//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
scanDecimation_(Parameters::defaultGridScanDecimation()),
cellSize_(Parameters::defaultGridCellSize()),
occupancyFromCloud_(Parameters::defaultGridFromDepth()),
projMapFrame_(Parameters::defaultGridMapFrameProjection()),
maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
normalKSearch_(Parameters::defaultGridNormalK()),
maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
minClusterSize_(Parameters::defaultGridMinClusterSize()),
flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
grid3D_(Parameters::defaultGrid3D()),
groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
xMin_(0.0f),
yMin_(0.0f)
{
this->parseParameters(parameters);
}
void OccupancyGrid::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
float cellSize = cellSize_;
if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
{
this->setCellSize(cellSize);
}
Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
if(maxGroundHeight_ > 0 &&
maxObstacleHeight_ > 0 &&
maxObstacleHeight_ < maxGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str(),
Parameters::kGridMaxObstacleHeight().c_str());
maxObstacleHeight_ = 0;
}
if(maxGroundHeight_ > 0 &&
minGroundHeight_ > 0 &&
maxGroundHeight_ < minGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
Parameters::kGridMinGroundHeight().c_str(),
Parameters::kGridMaxGroundHeight().c_str(),
Parameters::kGridMinGroundHeight().c_str());
minGroundHeight_ = 0;
}
Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
{
maxGroundAngle_ *= M_PI/180.0f;
}
Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
// convert ROI from string to vector
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
{
std::list<std::string> strValues = uSplit(iter->second, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
{
tmpValues[i] = uStr2Float(*jter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
roiRatios_ = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
}
}
}
if(maxGroundHeight_ <= 0.0f && !normalsSegmentation_)
{
UWARN("\"%s\" should be greater than 0 if not using normals "
"segmentation approach. Setting it to cell size (%f).",
Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
maxGroundHeight_ = cellSize_;
}
}
void OccupancyGrid::setCellSize(float cellSize)
{
UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
if(cellSize_ != cellSize)
{
if(!map_.empty())
{
UWARN("Grid cell size has changed, the map is cleared!");
}
this->clear();
cellSize_ = cellSize;
}
}
void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, 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);
if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
{
UDEBUG("2D laser scan");
//2D
util3d::occupancy2DFromLaserScan(
node.sensorData().laserScanRaw(),
ground,
obstacles,
cellSize_,
scan2dUnknownSpaceFilled_,
node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
}
else
{
// 3D
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(!occupancyFromCloud_)
{
UDEBUG("3D laser scan");
const Transform & t = node.sensorData().laserScanInfo().localTransform();
cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
cloud = util3d::laserScanToPointCloudRGB(
scan,
t);
// update viewpoint
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
}
else
{
UDEBUG("Depth image");
cloud = util3d::cloudRGBFromSensorData(
node.sensorData(),
cloudDecimation_,
cloudMaxDepth_,
cloudMinDepth_,
indices.get(),
parameters_,
roiRatios_);
// update viewpoint
if(node.sensorData().cameraModels().size())
{
// average of all local transforms
float sum = 0;
for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
{
const Transform & t = node.sensorData().cameraModels()[i].localTransform();
if(!t.isNull())
{
viewPoint.x += t.x();
viewPoint.y += t.y();
viewPoint.z += t.z();
sum += 1.0f;
}
}
if(sum > 0.0f)
{
viewPoint.x /= sum;
viewPoint.y /= sum;
viewPoint.z /= sum;
}
}
else
{
const Transform & t = node.sensorData().stereoCameraModel().localTransform();
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
}
}
if(cloud->size())
{
// voxelize to grid cell size
cloud = util3d::voxelize(cloud, indices, cellSize_);
indices->resize(cloud->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
indices->at(i) = i;
}
// add pose rotation without yaw
float roll, pitch, yaw;
node.getPose().getEulerAngles(roll, pitch, yaw);
UDEBUG("node.getPose()=%s projMapFrame_=%d", node.getPose().prettyPrint().c_str(), projMapFrame_?1:0);
cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
{
indices = util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
if(normalsSegmentation_)
{
UDEBUG("normalKSearch=%d", normalKSearch_);
UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", cellSize_*2.0f);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
cloud,
indices,
groundIndices,
obstaclesIndices,
normalKSearch_,
maxGroundAngle_,
cellSize_*2.0f,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,
0,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
}
else
{
UDEBUG("");
// passthrough filter
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
}
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
UDEBUG("");
if(groundIndices->size())
{
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(obstaclesIndices->size())
{
obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(groundIndices->empty() && obstaclesIndices->empty())
{
UWARN("Cloud (with %d points) is empty after noise "
"filtering. Occupancy grid of node %d cannot be "
"created.",
(int)cloud->size(), node.id());
return;
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
}
if(grid3D_)
{
UDEBUG("");
if(groundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
// transform back in base frame
Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
}
else
{
UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
cellSize_);
}
}
}
UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
}
void OccupancyGrid::clear()
{
cache_.clear();
map_ = cv::Mat();
mapInfo_ = cv::Mat();
cellCount_.clear();
xMin_ = 0.0f;
yMin_ = 0.0f;
addedNodes_.clear();
}
void OccupancyGrid::addToCache(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles)
{
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
{
UTimer timer;
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
float margin = cellSize_*10.0f+footprintRadius;
float minX=-minMapSize/2.0f;
float minY=-minMapSize/2.0f;
float maxX=minMapSize/2.0f;
float maxY=minMapSize/2.0f;
bool undefinedSize = minMapSize == 0.0f;
std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps;
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
bool graphChanged = false;
std::map<int, Transform> transforms;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
if(jter != posesIn.end())
{
UASSERT(!iter->second.isNull() && !jter->second.isNull());
Transform t = Transform::getIdentity();
if(iter->second.getDistanceSquared(jter->second) > 0.0001)
{
t = jter->second * iter->second.inverse();
graphChanged = true;
}
transforms.insert(std::make_pair(jter->first, t));
float x = jter->second.x();
float y =jter->second.y();
if(undefinedSize)
{
minX = maxX = x;
minY = maxY = y;
undefinedSize = false;
}
else
{
if(minX > x)
minX = x;
else if(maxX < x)
maxX = x;
if(minY > y)
minY = y;
else if(maxY < y)
maxY = y;
}
}
else
{
UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
}
}
if(graphChanged && !map_.empty())
{
UINFO("Graph changed!");
// 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols &&
map_.rows == mapInfo_.rows);
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
{
if(iter->second.first)
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
{
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
if(minX > pt.x)
minX = pt.x;
else if(maxX < pt.x)
maxX = pt.x;
if(minY > pt.y)
minY = pt.y;
else if(maxY < pt.y)
maxY = pt.y;
std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
if(map_.at<char>(y, x) == 0)
{
// ground
std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
UASSERT(iter != emptyLocalMaps.end());
UASSERT(jter->second.first < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.first++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
else
{
// obstacle
std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
UASSERT(iter != occupiedLocalMaps.end());
UASSERT(iter!=occupiedLocalMaps.end());
UASSERT(jter->second.second < iter->second.cols);
float * ptf = iter->second.ptr<float>(0,jter->second.second++);
ptf[0] = pt.x;
ptf[1] = pt.y;
}
}
}
}
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
addedNodes_.clear();
map_ = cv::Mat();
mapInfo_ = cv::Mat();
cellCount_.clear();
xMin_ = 0.0f;
yMin_ = 0.0f;
}
else if(!map_.empty())
{
// update
minX=xMin_+margin;
minY=yMin_+margin;
maxX=xMin_+float(map_.cols)*cellSize_ - margin;
maxY=yMin_+float(map_.rows)*cellSize_ - margin;
undefinedSize = false;
}
std::list<std::pair<int, Transform> > poses;
// place negative poses at the end
for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
{
if(iter->first>0)
{
poses.push_front(*iter);
}
else
{
poses.push_back(*iter);
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
float x = iter->second.x();
float y =iter->second.y();
if(undefinedSize)
{
minX = maxX = x;
minY = maxY = y;
undefinedSize = false;
}
else
{
if(minX > x)
minX = x;
else if(maxX < x)
maxX = x;
if(minY > y)
minY = y;
else if(maxY < y)
maxY = y;
}
}
if(!cache_.empty())
{
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(cache_, iter->first))
{
const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
//ground
if(pair.first.cols)
{
if(pair.first.rows > 1 && pair.first.cols == 1)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
}
cv::Mat ground(1, pair.first.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
{
const float * vi = pair.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
cv::Point3f vt;
if(pair.first.channels() > 2)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
else
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
}
vo[0] = vt.x;
vo[1] = vt.y;
if(minX > vo[0])
minX = vo[0];
else if(maxX < vo[0])
maxX = vo[0];
if(minY > vo[1])
minY = vo[1];
else if(maxY < vo[1])
maxY = vo[1];
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
}
//obstacles
if(pair.second.cols)
{
if(pair.second.rows > 1 && pair.second.cols == 1)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
}
cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
for(int i=0; i<obstacles.cols; ++i)
{
const float * vi = pair.second.ptr<float>(0,i);
float * vo = obstacles.ptr<float>(0,i);
cv::Point3f vt;
if(pair.second.channels() > 2)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
else
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
}
vo[0] = vt.x;
vo[1] = vt.y;
if(minX > vo[0])
minX = vo[0];
else if(maxX < vo[0])
maxX = vo[0];
if(minY > vo[1])
minY = vo[1];
else if(maxY < vo[1])
maxY = vo[1];
}
uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
}
}
}
}
cv::Mat map;
cv::Mat mapInfo;
if(minX != maxX && minY != maxY)
{
//Get map size
float xMin = minX-margin;
float yMin = minY-margin;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize_) > 99999 ||
fabs((xMax - xMin) / cellSize_) > 99999)
{
UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
"There's maybe an error with the poses provided! The map will not be created!",
xMin, yMin, xMax, yMax);
}
else
{
UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
if(map_.empty())
{
UDEBUG("Map empty!");
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
}
else
{
if(xMin == xMin_ && yMin == yMin_ &&
newMapSize.width == map_.cols &&
newMapSize.height == map_.rows)
{
// same map size and origin, don't do anything
UDEBUG("Map same size!");
map = map_;
mapInfo = mapInfo_;
}
else
{
UDEBUG("Copy map");
// copy the old map in the new map
// make sure the translation is cellSize
int deltaX = 0;
if(xMin < xMin_)
{
deltaX = (xMin_ - xMin) / cellSize_ + 1.0f;
xMin = xMin_-float(deltaX)*cellSize_;
}
int deltaY = 0;
if(yMin < yMin_)
{
deltaY = (yMin_ - yMin) / cellSize_ + 1.0f;
yMin = yMin_-float(deltaY)*cellSize_;
}
UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
}
}
UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
UDEBUG("map %d %d", map.cols, map.rows);
if(poses.size())
{
UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
}
for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
if(kter->first > 0)
{
uInsert(addedNodes_, *kter);
}
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
if(cter == cellCount_.end() && kter->first > 0)
{
cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
}
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.cols; ++i)
{
float * ptf = iter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId > 0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.first+=1;
}
value = 0; // free space
}
}
}
if(footprintRadius >= cellSize_*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
ptEnd.x = map.cols-1;
if(ptBegin.y < 0)
ptBegin.y = 0;
if(ptEnd.y >= map.rows)
ptEnd.y = map.rows-1;
for(int i=ptBegin.x; i<ptEnd.x; ++i)
{
for(int j=ptBegin.y; j<ptEnd.y; ++j)
{
UASSERT(j < map.rows && i < map.cols);
char & value = map.at<char>(j, i);
float * info = mapInfo.ptr<float>(j, i);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.first += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
cter->second.first+=1;
}
value = -2; // free space (footprint)
}
}
}
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.cols; ++i)
{
float * ptf = jter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
float * info = mapInfo.ptr<float>(pt.y, pt.x);
int nodeId = (int)info[0];
if(value != -1)
{
if(kter->first > 0 && (kter->first < nodeId || nodeId < 0))
{
// cannot rewrite on cells referred by more recent nodes
continue;
}
if(nodeId>0)
{
std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
if(value == 0)
{
eter->second.first -= 1;
}
else if(value == 100)
{
eter->second.second -= 1;
}
if(kter->first < 0)
{
eter->second.second += 1;
}
}
}
if(kter->first > 0)
{
info[0] = (float)kter->first;
info[1] = ptf[0];
info[2] = ptf[1];
cter->second.second+=1;
}
value = 100; // obstacles
}
}
}
}
// fill holes and put footprint values to empty (0)
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//cloud->resize(map.rows*map.cols);
//int oi=0;
for(int i=1; i<map.rows-1; ++i)
{
for(int j=1; j<map.cols-1; ++j)
{
char & value = map.at<char>(i, j);
if(value == -2)
{
value = 0;
}
char sum = (map.at<char>(i+1, j) != -1?1:0) +
(map.at<char>(i-1, j) != -1?1:0) +
(map.at<char>(i, j+1) != -1?1:0) +
(map.at<char>(i, j-1) != -1?1:0);
if(value == -1 && sum >=3)
{
value = 0;
}
//float * info = mapInfo.ptr<float>(i,j);
//if(info[0] > 0)
//{
// cloud->at(oi).x = info[1];
// cloud->at(oi).y = info[2];
// oi++;
//}
}
}
//if(graphChanged)
//{
// cloud->resize(oi);
// pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud);
// UWARN("Saved mapInfo.pcd");
//}
map_ = map;
mapInfo_ = mapInfo;
xMin_ = xMin;
yMin_ = yMin;
// clean cellCount_
for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
{
UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
if(iter->second.first == 0 && iter->second.second == 0)
{
cellCount_.erase(iter++);
}
else
{
++iter;
}
}
}
}
cache_.clear();
UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
}
}

82
corelib/src/OctoMap.cpp
View File

@@ -31,11 +31,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <pcl/common/transforms.h>
namespace rtabmap {
OctoMap::OctoMap(float voxelSize) :
octree_(new octomap::ColorOcTree(voxelSize))
octree_(new octomap::ColorOcTree(voxelSize)),
hasColor_(false)
{
UASSERT(voxelSize>0.0f);
}
@@ -51,16 +53,32 @@ void OctoMap::clear()
octree_->clear();
occupiedCells_.clear();
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false;
}
void OctoMap::addToCache(int nodeId,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
const pcl::PointXYZ & viewPoint)
{
UDEBUG("nodeId=%d", nodeId);
cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
}
void OctoMap::addToCache(int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Point3f & viewPoint)
{
UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
UDEBUG("nodeId=%d", nodeId);
cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
}
void OctoMap::update(const std::map<int, Transform> & poses)
@@ -174,12 +192,19 @@ void OctoMap::update(const std::map<int, Transform> & poses)
for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
{
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
cloudIter = cache_.find(iter->first);
if(cloudIter != cache_.end())
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
viewPointIter = cacheViewPoints_.find(iter->first);
if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
{
UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
UASSERT(viewPointIter != cacheViewPoints_.end());
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -190,10 +215,21 @@ void OctoMap::update(const std::map<int, Transform> & poses)
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
// insert ground points only as free:
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)cloudIter->second.first->size());
for (unsigned int i=0; i<cloudIter->second.first->size(); ++i)
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.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();
for (unsigned int i=0; i<maxGroundPts; ++i)
{
pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.first->at(i), iter->second);
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
@@ -211,6 +247,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
@@ -226,10 +266,20 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int) cloudIter->second.second->size());
for (unsigned int i=0; i<cloudIter->second.second->size(); ++i)
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.second->at(i), iter->second);
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
}
else
{
pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
}
octomap::point3d point(pt.x, pt.y, pt.z);
@@ -247,6 +297,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
{
hasColor_ = true;
}
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
@@ -298,6 +352,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
}
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
}
void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -385,7 +441,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
if(octree_->isNodeOccupied(*it))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->getTreeDepth() == it.getDepth())
if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
{
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
}
@@ -475,14 +531,14 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
ground = util3d::voxelize(ground, gridCellSize);
}
cv::Mat obstaclesMat = cv::Mat((int)obstacles->size(), 1, CV_32FC2);
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
for(unsigned int i=0;i<obstacles->size(); ++i)
{
obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
}
cv::Mat groundMat = cv::Mat((int)ground->size(), 1, CV_32FC2);
cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
for(unsigned int i=0;i<ground->size(); ++i)
{
groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;

8
corelib/src/OdometryF2F.cpp
View File

@@ -133,7 +133,7 @@ Transform OdometryF2F::computeTransform(
info->words = newFrame.getWords();
info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols;
info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t);
info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), tmpRefFrame.sensorData().laserScanInfo().localTransform()*t);
}
}
else
@@ -168,7 +168,7 @@ Transform OdometryF2F::computeTransform(
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
(registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
(newFrame.sensorData().laserScanRaw().cols &&
(newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
(newFrame.sensorData().laserScanInfo().maxPoints() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
{
refFrame_ = newFrame;
@@ -196,9 +196,9 @@ Transform OdometryF2F::computeTransform(
{
UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanInfo().maxPoints() != 0 && 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().cols)/float(newFrame.sensorData().laserScanMaxPts()), 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());
}
}
}

8
corelib/src/OdometryF2M.cpp
View File

@@ -334,7 +334,7 @@ Transform OdometryF2M::computeTransform(
if(lastFrame_->sensorData().laserScanRaw().cols)
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose);
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
@@ -444,7 +444,7 @@ Transform OdometryF2M::computeTransform(
{
*map_ = tmpMap;
map_->sensorData().setLaserScanRaw(mapScan, 0, 0);
map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
map_->setWords(mapWords);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
@@ -534,9 +534,9 @@ Transform OdometryF2M::computeTransform(
frameValid = true;
if (fixedMapPath_.empty())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose);
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
}
}
else

1
corelib/src/OdometryThread.cpp
View File

@@ -97,6 +97,7 @@ void OdometryThread::mainLoop()
Transform pose = _odometry->process(data, &info);
// a null pose notify that odometry could not be computed
double variance = info.variance>0?info.variance:1;
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
this->post(new OdometryEvent(data, pose, variance, variance, info));
}
}

27
corelib/src/Parameters.cpp
View File

@@ -220,6 +220,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.11.10 typos
removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected", std::make_pair(true, Parameters::kGridFlatObstacleDetected())));
// 0.11.8
removedParameters_.insert(std::make_pair("Reg/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("OdomF2M/ScanSubstractRadius", std::make_pair(true, Parameters::kOdomF2MScanSubtractRadius())));
@@ -402,53 +405,65 @@ std::string Parameters::getDescription(const std::string & paramKey)
return description;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Bool(iter->second.c_str());
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Int(iter->second.c_str());
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Int(iter->second.c_str());
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Float(iter->second);
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = uStr2Double(iter->second);
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
bool Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
{
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = iter->second;
return true;
}
return false;
}
void Parameters::parse(const ParametersMap & parameters, ParametersMap & parametersOut)
{

22
corelib/src/RegistrationIcp.cpp
View File

@@ -113,10 +113,12 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
// ICP with guess transform
int maxLaserScansTo = dataTo.laserScanMaxPts();
int maxLaserScansFrom = dataFrom.laserScanMaxPts();
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();
if(_downsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _downsamplingStep);
@@ -140,8 +142,8 @@ Transform RegistrationIcp::computeTransformationImpl(
toScan.channels() == 6)
{
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, toLocalTransform * guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -180,8 +182,8 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, toLocalTransform * guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
@@ -223,8 +225,8 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
UDEBUG("Compute normals time = %f s", timer.ticks());
@@ -259,8 +261,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_voxelSize > 0.0f)
{
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
icpT = util3d::icp(

13
corelib/src/Rtabmap.cpp
View File

@@ -413,8 +413,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths);
Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
_proximityAngle *= M_PI/180.0f;
if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle))
{
_proximityAngle *= M_PI/180.0f;
}
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
@@ -2386,7 +2388,7 @@ bool Rtabmap::process(
signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
if(!_someNodesHaveBeenTransferred && transferred.size())
{
_someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization
_someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
}
}
_lastProcessTime = totalTime;
@@ -2502,6 +2504,7 @@ bool Rtabmap::process(
UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
}
UDEBUG("");
// Set local graph
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
@@ -2516,6 +2519,7 @@ bool Rtabmap::process(
poses = _optimizedPoses;
constraints = _constraints;
}
UDEBUG("Get all node infos...");
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform odomPose;
@@ -2540,15 +2544,18 @@ bool Rtabmap::process(
statistics_.setSignatures(signatures);
statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
localGraphSize = (int)poses.size();
UDEBUG("");
}
//Start trashing
UDEBUG("Empty trash...");
_memory->emptyTrash();
// Log info...
// TODO : use a specific class which will handle the RtabmapEvent
if(_foutFloat && _foutInt)
{
UDEBUG("Logging...");
std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
totalTime,
timeMemoryUpdate,

215
corelib/src/SensorData.cpp
View File

@@ -38,8 +38,7 @@ namespace rtabmap
SensorData::SensorData() :
_id(0),
_stamp(0.0),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f)
_cellSize(0.0f)
{
}
@@ -51,8 +50,7 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f)
_cellSize(0.0f)
{
if(image.rows == 1)
{
@@ -85,9 +83,8 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f),
_cameraModels(std::vector<CameraModel>(1, cameraModel))
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_cellSize(0.0f)
{
if(image.rows == 1)
{
@@ -121,9 +118,8 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f),
_cameraModels(std::vector<CameraModel>(1, cameraModel))
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_cellSize(0.0f)
{
if(rgb.rows == 1)
{
@@ -162,8 +158,7 @@ SensorData::SensorData(
// RGB-D constructor + laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & rgb,
const cv::Mat & depth,
const CameraModel & cameraModel,
@@ -172,9 +167,9 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_laserScanMaxRange(laserScanMaxRange),
_cameraModels(std::vector<CameraModel>(1, cameraModel))
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(rgb.rows == 1)
{
@@ -199,7 +194,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
{
_laserScanRaw = laserScan;
}
@@ -229,9 +224,8 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f),
_cameraModels(cameraModels)
_cameraModels(cameraModels),
_cellSize(0.0f)
{
if(rgb.rows == 1)
{
@@ -269,8 +263,7 @@ SensorData::SensorData(
// Multi-cameras RGB-D constructor + laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
@@ -279,9 +272,9 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_laserScanMaxRange(laserScanMaxRange),
_cameraModels(cameraModels)
_cameraModels(cameraModels),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(rgb.rows == 1)
{
@@ -306,7 +299,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
{
_laserScanRaw = laserScan;
}
@@ -336,9 +329,8 @@ SensorData::SensorData(
const cv::Mat & userData):
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f),
_stereoCameraModel(cameraModel)
_stereoCameraModel(cameraModel),
_cellSize(0.0f)
{
if(left.rows == 1)
{
@@ -378,8 +370,7 @@ SensorData::SensorData(
// Stereo constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const LaserScanInfo & laserScanInfo,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
@@ -388,9 +379,9 @@ SensorData::SensorData(
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_laserScanMaxRange(laserScanMaxRange),
_stereoCameraModel(cameraModel)
_stereoCameraModel(cameraModel),
_laserScanInfo(laserScanInfo),
_cellSize(0.0f)
{
if(left.rows == 1)
{
@@ -414,7 +405,7 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
{
_laserScanRaw = laserScan;
}
@@ -480,18 +471,97 @@ void SensorData::setUserData(const cv::Mat & userData)
}
}
void SensorData::setOccupancyGrid(
const cv::Mat & ground,
const cv::Mat & obstacles,
float cellSize,
const cv::Point3f & viewPoint)
{
UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
{
UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
return;
}
_groundCellsRaw = cv::Mat();
_groundCellsCompressed = cv::Mat();
_obstacleCellsRaw = cv::Mat();
_obstacleCellsCompressed = cv::Mat();
CompressionThread ctGround(ground);
CompressionThread ctObstacles(obstacles);
if(!ground.empty())
{
if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6))
{
_groundCellsRaw = ground;
ctGround.start();
}
else if(ground.type() == CV_8UC1)
{
UASSERT(ground.type() == CV_8UC1); // Bytes
_groundCellsCompressed = ground;
}
}
if(!obstacles.empty())
{
if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6))
{
_obstacleCellsRaw = obstacles;
ctObstacles.start();
}
else if(obstacles.type() == CV_8UC1)
{
UASSERT(obstacles.type() == CV_8UC1); // Bytes
_obstacleCellsCompressed = obstacles;
}
}
ctGround.join();
ctObstacles.join();
if(!_groundCellsRaw.empty())
{
_groundCellsCompressed = ctGround.getCompressedData();
}
if(!_obstacleCellsRaw.empty())
{
_obstacleCellsCompressed = ctObstacles.getCompressedData();
}
_cellSize = cellSize;
_viewPoint = viewPoint;
}
void SensorData::uncompressData()
{
cv::Mat tmpA, tmpB, tmpC, tmpD;
cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
uncompressData(_imageCompressed.empty()?0:&tmpA,
_depthOrRightCompressed.empty()?0:&tmpB,
_laserScanCompressed.empty()?0:&tmpC,
_userDataCompressed.empty()?0:&tmpD);
_userDataCompressed.empty()?0:&tmpD,
_groundCellsCompressed.empty()?0:&tmpE,
_obstacleCellsCompressed.empty()?0:&tmpF);
}
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
void SensorData::uncompressData(
cv::Mat * imageRaw,
cv::Mat * depthRaw,
cv::Mat * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw)
{
uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
UDEBUG("%d", this->id());
uncompressDataConst(
imageRaw,
depthRaw,
laserScanRaw,
userDataRaw,
groundCellsRaw,
obstacleCellsRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
@@ -520,9 +590,23 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
{
_userDataRaw = *userDataRaw;
}
if(groundCellsRaw && !groundCellsRaw->empty() && _groundCellsRaw.empty())
{
_groundCellsRaw = *groundCellsRaw;
}
if(obstacleCellsRaw && !obstacleCellsRaw->empty() && _obstacleCellsRaw.empty())
{
_obstacleCellsRaw = *obstacleCellsRaw;
}
}
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
void SensorData::uncompressDataConst(
cv::Mat * imageRaw,
cv::Mat * depthRaw,
cv::Mat * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw) const
{
if(imageRaw)
{
@@ -540,35 +624,64 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
{
*userDataRaw = _userDataRaw;
}
if(groundCellsRaw)
{
*groundCellsRaw = _groundCellsRaw;
}
if(obstacleCellsRaw)
{
*obstacleCellsRaw = _obstacleCellsRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()) ||
(userDataRaw && userDataRaw->empty()))
(userDataRaw && userDataRaw->empty()) ||
(groundCellsRaw && groundCellsRaw->empty()) ||
(obstacleCellsRaw && obstacleCellsRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
if(imageRaw && imageRaw->empty())
rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
{
UASSERT(_imageCompressed.type() == CV_8UC1);
ctImage.start();
}
if(depthRaw && depthRaw->empty())
if(depthRaw && depthRaw->empty() && !_depthOrRightCompressed.empty())
{
UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
ctDepth.start();
}
if(laserScanRaw && laserScanRaw->empty())
if(laserScanRaw && laserScanRaw->empty() && !_laserScanCompressed.empty())
{
UASSERT(_laserScanCompressed.type() == CV_8UC1);
ctLaserScan.start();
}
if(userDataRaw && userDataRaw->empty())
if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
{
UASSERT(_userDataCompressed.type() == CV_8UC1);
ctUserData.start();
}
if(groundCellsRaw && groundCellsRaw->empty() && !_groundCellsCompressed.empty())
{
UASSERT(_groundCellsCompressed.type() == CV_8UC1);
ctGroundCells.start();
}
if(obstacleCellsRaw && obstacleCellsRaw->empty() && !_obstacleCellsCompressed.empty())
{
UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
ctObstacleCells.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
ctUserData.join();
ctGroundCells.join();
ctObstacleCells.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
@@ -603,6 +716,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
}
}
if(groundCellsRaw && groundCellsRaw->empty())
{
*groundCellsRaw = ctGroundCells.getUncompressedData();
}
if(obstacleCellsRaw && obstacleCellsRaw->empty())
{
*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
}
}
}
@@ -615,7 +736,11 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_userDataCompressed.total()*_userDataCompressed.elemSize() +
_userDataRaw.total()*_userDataRaw.elemSize() +
_laserScanCompressed.total()*_laserScanCompressed.elemSize() +
_laserScanRaw.total()*_laserScanRaw.elemSize();
_laserScanRaw.total()*_laserScanRaw.elemSize() +
_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize();
}
} // namespace rtabmap

5
corelib/src/Signature.cpp
View File

@@ -44,8 +44,7 @@ Signature::Signature() :
_saved(false),
_modified(true),
_linksModified(true),
_enabled(false),
_cellSize(0.0f)
_enabled(false)
{
}
@@ -69,7 +68,6 @@ Signature::Signature(
_enabled(false),
_pose(pose),
_groundTruthPose(groundTruthPose),
_cellSize(0.0f),
_sensorData(sensorData)
{
if(_sensorData.id() == 0)
@@ -91,7 +89,6 @@ Signature::Signature(const SensorData & data) :
_enabled(false),
_pose(Transform::getIdentity()),
_groundTruthPose(data.groundTruth()),
_cellSize(0.0f),
_sensorData(data)
{

6
corelib/src/StereoCameraModel.cpp
View File

@@ -344,6 +344,12 @@ void StereoCameraModel::scale(double scale)
right_ = right_.scaled(scale);
}
void StereoCameraModel::roi(const cv::Rect & roi)
{
left_ = left_.roi(roi);
right_ = right_.roi(roi);
}
float StereoCameraModel::computeDepth(float disparity) const
{
//depth = baseline * f / (disparity + cx1-cx0);

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

@@ -21,9 +21,7 @@ CREATE TABLE Node (
pose BLOB,
ground_truth_pose BLOB,
label TEXT,
obstacle_cells BLOB,
ground_cells BLOB,
cell_size FLOAT,
time_enter DATE,
PRIMARY KEY (id)
);
@@ -33,9 +31,17 @@ CREATE TABLE Data (
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_max_pts INTEGER, -- Laser scan max points
scan_max_range FLOAT, -- Laser max range
scan_info BLOB, -- scan_max_pts, scan_max_range, local_transform
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)
cell_size FLOAT,
view_point_x FLOAT,
view_point_y FLOAT,
view_point_z FLOAT,
user_data BLOB, -- compressed data (User data)
time_enter DATE,
PRIMARY KEY (id)

86
corelib/src/util2d.cpp
View File

@@ -1089,6 +1089,92 @@ float getDepth(
return depth;
}
cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios)
{
return computeRoi(image.size(), roiRatios);
}
cv::Rect computeRoi(const cv::Size & imageSize, const std::string & roiRatios)
{
std::list<std::string> strValues = uSplit(roiRatios, ' ');
if(strValues.size() != 4)
{
UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
}
else
{
std::vector<float> values(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
values[i] = uStr2Float(*iter);
++i;
}
if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
{
return computeRoi(imageSize, values);
}
else
{
UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
}
}
return cv::Rect();
}
cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
return computeRoi(image.size(), roiRatios);
}
cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios)
{
if(imageSize.height!=0 && imageSize.width!= 0 && roiRatios.size() == 4)
{
float width = imageSize.width;
float height = imageSize.height;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
{
roi.x = width * roiRatios[0];
}
//right roi
if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
{
roi.width -= width * roiRatios[1] + width * roiRatios[0];
}
//top roi
if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
{
roi.y = height * roiRatios[2];
}
//bottom roi
if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
{
roi.height -= height * roiRatios[3] + height * roiRatios[2];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
return cv::Rect();
}
}
cv::Mat decimate(const cv::Mat & image, int decimation)
{
UASSERT(decimation >= 1);

283
corelib/src/util3d.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
@@ -699,7 +700,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters)
const ParametersMap & parameters,
const std::vector<float> & roiRatios)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -712,9 +714,46 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
{
if(sensorData.cameraModels()[i].isValidForProjection())
{
cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
CameraModel model = sensorData.cameraModels()[i];
if( roiRatios.size() == 4 &&
roiRatios[0] != 0.0f &&
roiRatios[1] != 0.0f &&
roiRatios[2] != 0.0f &&
roiRatios[3] != 0.0f)
{
if( int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
(model.imageWidth() == 0 ||
model.imageHeight() == 0 ||
(int((roiRatios[0]+roiRatios[1])*double(model.imageWidth()))%decimation==0 &&
int((roiRatios[2]+roiRatios[3])*double(model.imageHeight()))%decimation==0)))
{
cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
depth = cv::Mat(depth, roiDepth);
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
}
else
{
model = model.roi(roiDepth);
}
}
else
{
UWARN("Cannot apply ROI ratios because resulting "
"dimension (%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
decimation);
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i],
depth,
model,
decimation,
maxDepth,
minDepth,
@@ -722,7 +761,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
tmp = util3d::transformPointCloud(tmp, model.localTransform());
if(sensorData.cameraModels().size() > 1)
{
@@ -765,6 +804,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
{
leftMono = sensorData.imageRaw();
}
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
sensorData.stereoCameraModel(),
@@ -790,7 +830,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float maxDepth,
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & parameters)
const ParametersMap & parameters,
const std::vector<float> & roiRatios)
{
UASSERT(!sensorData.imageRaw().empty());
UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -822,10 +863,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
{
if(sensorData.cameraModels()[i].isValidForProjection())
{
cv::Mat depth(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
cv::Mat rgb(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
CameraModel model = sensorData.cameraModels()[i];
if( roiRatios.size() == 4 &&
roiRatios[0] != 0.0f &&
roiRatios[1] != 0.0f &&
roiRatios[2] != 0.0f &&
roiRatios[3] != 0.0f)
{
if( int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
int((roiRatios[0]+roiRatios[1])*double(rgb.cols))%decimation==0 &&
int((roiRatios[2]+roiRatios[3])*double(rgb.rows))%decimation==0)
{
cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
depth = cv::Mat(depth, roiDepth);
rgb = cv::Mat(rgb, roiRgb);
model = model.roi(roiRgb);
}
else
{
UWARN("Cannot apply ROI ratios because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
int((roiRatios[0]+roiRatios[1])*double(rgb.cols)),
int((roiRatios[2]+roiRatios[3])*double(rgb.rows)),
decimation);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i],
depth,
rgb,
model,
decimation,
maxDepth,
minDepth,
@@ -833,7 +907,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
tmp = util3d::transformPointCloud(tmp, model.localTransform());
if(sensorData.cameraModels().size() > 1)
{
@@ -866,7 +940,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
{
//stereo
UDEBUG("");
cloud = cloudFromStereoImages(sensorData.imageRaw(),
cloud = cloudFromStereoImages(
sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel(),
decimation,
@@ -973,6 +1048,31 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(4));
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
if(!nullTransform)
{
pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec4f>(i)[0] = pt.x;
laserScan.at<cv::Vec4f>(i)[1] = pt.y;
laserScan.at<cv::Vec4f>(i)[2] = pt.z;
}
else
{
laserScan.at<cv::Vec4f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec4f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec4f>(i)[2] = cloud.at(i).z;
}
laserScan.at<cv::Vec4i>(i)[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -998,7 +1098,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
@@ -1006,24 +1106,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
if(laserScan.type() == CV_32FC2)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
}
else
{
output->at(i).x = laserScan.at<cv::Vec6f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec6f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec6f>(i)[2];
}
output->at(i) = util3d::laserScanToPoint(laserScan, i);
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
@@ -1034,34 +1117,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull();
for(int i=0; i<laserScan.cols; ++i)
{
if(laserScan.type() == CV_32FC2)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
}
else
{
output->at(i).x = laserScan.at<cv::Vec6f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec6f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec6f>(i)[2];
output->at(i).normal_x = laserScan.at<cv::Vec6f>(i)[3];
output->at(i).normal_y = laserScan.at<cv::Vec6f>(i)[4];
output->at(i).normal_z = laserScan.at<cv::Vec6f>(i)[5];
}
output->at(i) = laserScanToPointNormal(laserScan, i);
if(!nullTransform)
{
output->at(i) = util3d::transformPoint(output->at(i), transform);
@@ -1070,6 +1133,124 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i) = util3d::laserScanToPointRGB(laserScan, i);
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
}
}
return output;
}
pcl::PointXYZ laserScanToPoint(const cv::Mat & 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(6));
pcl::PointXYZ output;
if(laserScan.type() == CV_32FC2)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
}
return output;
}
pcl::PointNormal laserScanToPointNormal(const cv::Mat & 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(6));
pcl::PointNormal output;
if(laserScan.type() == CV_32FC2)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
output.normal_x = laserScan.at<cv::Vec6f>(index)[3];
output.normal_y = laserScan.at<cv::Vec6f>(index)[4];
output.normal_z = laserScan.at<cv::Vec6f>(index)[5];
}
return output;
}
pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & 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(6));
pcl::PointXYZRGB output;
if(laserScan.type() == CV_32FC2)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
output.b = (unsigned char)(laserScan.at<cv::Vec4i>(index)[3] & 0xFF);
output.g = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 8) & 0xFF);
output.r = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 16) & 0xFF);
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
}
return output;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,

121
corelib/src/util3d_mapping.cpp
View File

@@ -84,7 +84,7 @@ void occupancy2DFromLaserScan(
ground = cv::Mat();
if(groundIndices.size())
{
ground = cv::Mat((int)groundIndices.size(), 1, CV_32FC2);
ground = cv::Mat(1, (int)groundIndices.size(), CV_32FC2);
int i=0;
for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
{
@@ -100,7 +100,7 @@ void occupancy2DFromLaserScan(
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
@@ -155,14 +155,12 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
bool undefinedSize = minMapSize == 0.0f;
float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
cv::Mat affineTransform(2,3,CV_32FC1);
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
float x = iter->second.x();
float y =iter->second.y();
if(undefinedSize)
{
minX = maxX = x;
@@ -185,53 +183,75 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
if(uContains(occupancy, iter->first))
{
const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
cosT = cos(yaw);
sinT = sin(yaw);
affineTransform.at<float>(0,0) = cosT;
affineTransform.at<float>(0,1) = -sinT;
affineTransform.at<float>(1,0) = sinT;
affineTransform.at<float>(1,1) = cosT;
affineTransform.at<float>(0,2) = x;
affineTransform.at<float>(1,2) = y;
//ground
if(pair.first.rows)
if(pair.first.cols)
{
UASSERT(pair.first.type() == CV_32FC2);
cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
cv::transform(pair.first, ground, affineTransform);
for(int i=0; i<ground.rows; ++i)
if(pair.first.rows > 1 && pair.first.cols == 1)
{
if(minX > ground.at<float>(i,0))
minX = ground.at<float>(i,0);
else if(maxX < ground.at<float>(i,0))
maxX = ground.at<float>(i,0);
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
}
cv::Mat ground(1, pair.first.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
{
const float * vi = pair.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
cv::Point3f vt;
if(pair.first.channels() > 2)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
else
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
}
vo[0] = vt.x;
vo[1] = vt.y;
if(minX > vo[0])
minX = vo[0];
else if(maxX < vo[0])
maxX = vo[0];
if(minY > ground.at<float>(i,1))
minY = ground.at<float>(i,1);
else if(maxY < ground.at<float>(i,1))
maxY = ground.at<float>(i,1);
if(minY > vo[1])
minY = vo[1];
else if(maxY < vo[1])
maxY = vo[1];
}
emptyLocalMaps.insert(std::make_pair(iter->first, ground));
}
//obstacles
if(pair.second.rows)
if(pair.second.cols)
{
UASSERT(pair.second.type() == CV_32FC2);
cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
cv::transform(pair.second, obstacles, affineTransform);
for(int i=0; i<obstacles.rows; ++i)
if(pair.second.rows > 1 && pair.second.cols == 1)
{
if(minX > obstacles.at<float>(i,0))
minX = obstacles.at<float>(i,0);
else if(maxX < obstacles.at<float>(i,0))
maxX = obstacles.at<float>(i,0);
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
}
cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
for(int i=0; i<obstacles.cols; ++i)
{
const float * vi = pair.second.ptr<float>(0,i);
float * vo = obstacles.ptr<float>(0,i);
cv::Point3f vt;
if(pair.second.channels() > 2)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
else
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
}
vo[0] = vt.x;
vo[1] = vt.y;
if(minX > vo[0])
minX = vo[0];
else if(maxX < vo[0])
maxX = vo[0];
if(minY > obstacles.at<float>(i,1))
minY = obstacles.at<float>(i,1);
else if(maxY < obstacles.at<float>(i,1))
maxY = obstacles.at<float>(i,1);
if(minY > vo[1])
minY = vo[1];
else if(maxY < vo[1])
maxY = vo[1];
}
occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
}
@@ -267,12 +287,14 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.rows; ++i)
for(int i=0; i<iter->second.cols; ++i)
{
cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
if(map.at<char>(pt.y, pt.x) != -2)
float * ptf = iter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
map.at<char>(pt.y, pt.x) = 0; // free space
value = 0; // free space
}
}
}
@@ -302,12 +324,14 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.rows; ++i)
for(int i=0; i<jter->second.cols; ++i)
{
cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
if(map.at<char>(pt.y, pt.x) != -2)
float * ptf = jter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
map.at<char>(pt.y, pt.x) = 100; // obstacles
value = 100; // obstacles
}
}
}
@@ -412,6 +436,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
}
}
}
UDEBUG("timer=%fs", timer.ticks());
return map;
}

13
corelib/src/util3d_transforms.cpp
View File

@@ -38,7 +38,7 @@ namespace util3d
cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
cv::Mat output = laserScan.clone();
@@ -66,6 +66,17 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
output.at<cv::Vec3f>(i)[1] = pt.y;
output.at<cv::Vec3f>(i)[2] = pt.z;
}
else if(laserScan.type() == CV_32FC(4))
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec4f>(i)[0],
laserScan.at<cv::Vec4f>(i)[1],
laserScan.at<cv::Vec4f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec4f>(i)[0] = pt.x;
output.at<cv::Vec4f>(i)[1] = pt.y;
output.at<cv::Vec4f>(i)[2] = pt.z;
}
else
{
pcl::PointNormal pt;