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

0.16.0: Database updated with Data.empty_cells, Admin.opt_map, Admin.opt_map_x_min and Admin.opt_map_y_min fields. Changed Parameter Grid/ProjRayTracing to Grid/RayTracing (OctoMap ray tracing done for 3D local grids). Improved OctoMap performance.

Browse Source
This commit is contained in:
matlabbe
2018-02-08 21:40:17 -05:00
parent e7ceacc215
commit fced2c521c
32 changed files with 1642 additions and 600 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -92,6 +92,7 @@ public:
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint);
void updateDepthImage(int nodeId, const cv::Mat & image);
@@ -216,6 +217,7 @@ private:
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const = 0;

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

@@ -48,6 +48,7 @@ public:
float getMinMapSize() const {return minMapSize_;}
bool isGridFromDepth() const {return occupancyFromCloud_;}
bool isFullUpdate() const {return fullUpdate_;}
float getUpdateError() const {return updateError_;}
bool isMapFrameProjection() const {return projMapFrame_;}
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
int cacheSize() const {return (int)cache_.size();}
@@ -64,19 +65,38 @@ public:
void createLocalMap(
const Signature & node,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPoint) const;
void createLocalMap(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const;
void createLocalMap(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
const pcl::IndicesPtr & indices,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const;
void clear();
void addToCache(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles);
const cv::Mat & obstacles,
const cv::Mat & empty);
void update(const std::map<int, Transform> & poses);
cv::Mat getMap(float & xMin, float & yMin) const;
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
private:
ParametersMap parameters_;
@@ -106,13 +126,14 @@ private:
int noiseFilteringMinNeighbors_;
bool scan2dUnknownSpaceFilled_;
double scan2dMaxUnknownSpaceFilledRange_;
bool projRayTracing_;
bool rayTracing_;
bool fullUpdate_;
float minMapSize_;
bool erode_;
float footprintRadius_;
float updateError_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_; //<node id, <ground, obstacles> >
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
cv::Mat map_;
cv::Mat mapInfo_;
std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
@@ -123,6 +144,7 @@ private:
bool cloudAssembling_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
};
}

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

@@ -44,22 +44,132 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class OcTreeNodeInfo
// forward declaraton for "friend"
class RtabmapColorOcTree;
class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
{
public:
OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
nodeRefId_(nodeRefId),
key_(key),
isObstacle_(isObstacle) {}
enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
public:
friend class RtabmapColorOcTree; // needs access to node children (inherited)
RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(-1) {}
RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
void setOccupancyType(char type) {type_=type;}
void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
int getNodeRefId() const {return nodeRefId_;}
char getOccupancyType() const {return type_;}
const octomap::point3d & getPointRef() const {return pointRef_;}
private:
int nodeRefId_;
octomap::OcTreeKey key_;
bool isObstacle_;
char type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
octomap::point3d pointRef_;
};
// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
public:
/// Default constructor, sets resolution of leafs
RtabmapColorOcTree(double resolution);
/// virtual constructor: creates a new object of same type
/// (Covariant return type requires an up-to-date compiler)
RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
/**
* Prunes a node when it is collapsible. This overloaded
* version only considers the node occupancy for pruning,
* different colors of child nodes are ignored.
* @return true if pruning was successful
*/
virtual bool pruneNode(RtabmapColorOcTreeNode* node);
virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
// set node color at given key or coordinate. Replaces previous color.
RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
uint8_t g, uint8_t b);
RtabmapColorOcTreeNode* setNodeColor(float x, float y,
float z, uint8_t r,
uint8_t g, uint8_t b) {
octomap::OcTreeKey key;
if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
return setNodeColor(key,r,g,b);
}
// integrate color measurement at given key or coordinate. Average with previous color
RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
uint8_t g, uint8_t b);
RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
float z, uint8_t r,
uint8_t g, uint8_t b) {
octomap:: OcTreeKey key;
if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
return averageNodeColor(key,r,g,b);
}
// integrate color measurement at given key or coordinate. Average with previous color
RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
uint8_t g, uint8_t b);
RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
float z, uint8_t r,
uint8_t g, uint8_t b) {
octomap::OcTreeKey key;
if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
return integrateNodeColor(key,r,g,b);
}
// update inner nodes, sets color to average child color
void updateInnerOccupancy();
protected:
void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
/**
* Static member object which ensures that this OcTree's prototype
* ends up in the classIDMapping only once. You need this as a
* static member in any derived octree class in order to read .ot
* files through the AbstractOcTree factory. You should also call
* ensureLinking() once from the constructor.
*/
class StaticMemberInitializer{
public:
StaticMemberInitializer() {
RtabmapColorOcTree* tree = new RtabmapColorOcTree(0.1);
tree->clearKeyRays();
AbstractOcTree::registerTreeType(tree);
}
/**
* Dummy function to ensure that MSVC does not drop the
* StaticMemberInitializer, causing this tree failing to register.
* Needs to be called from the constructor of this octree.
*/
void ensureLinking() {};
};
/// static member to ensure static initialization (only once)
static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
};
class RTABMAP_EXP OctoMap {
public:
static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
public:
OctoMap(const ParametersMap & parameters, float occupancyThr = 0.5f);
OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,
@@ -69,15 +179,18 @@ public:
void addToCache(int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
const cv::Point3f & viewPoint);
void update(const std::map<int, Transform> & poses);
const octomap::ColorOcTree * octree() const {return octree_;}
const RtabmapColorOcTree * octree() const {return octree_;}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
unsigned int treeDepth = 0,
std::vector<int> * obstacleIndices = 0,
std::vector<int> * emptyIndices = 0) const;
std::vector<int> * emptyIndices = 0,
std::vector<int> * groundIndices = 0,
bool originalRefPoints = true) const;
cv::Mat createProjectionMap(
float & xMin,
@@ -91,16 +204,24 @@ public:
virtual ~OctoMap();
void clear();
void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
private:
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_;
void updateMinMax(const octomap::point3d & point);
private:
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
std::map<int, cv::Point3f> cacheViewPoints_;
octomap::ColorOcTree * octree_;
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
RtabmapColorOcTree * octree_;
std::map<int, Transform> addedNodes_;
octomap::KeyRay keyRay_;
bool hasColor_;
bool fullUpdate_;
float updateError_;
double minValues_[3];
double maxValues_[3];
};
} /* namespace rtabmap */

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

@@ -617,9 +617,10 @@ class RTABMAP_EXP Parameters
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, 4.0, "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
RTABMAP_PARAM(Grid, ProjRayTracing, bool, true, uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
RTABMAP_PARAM(Grid, RayTracing, bool, false, uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
RTABMAP_PARAM(GridGlobal, FullUpdate, bool, true, "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
RTABMAP_PARAM(GridGlobal, UpdateError, float, 0.01, "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
RTABMAP_PARAM(GridGlobal, FootprintRadius, float, 0.0, "Footprint radius (m) used to clear all obstacles under the graph.");
RTABMAP_PARAM(GridGlobal, MinSize, float, 0.0, "Minimum map size (m).");
RTABMAP_PARAM(GridGlobal, Eroded, bool, false, "Erode obstacle cells.");

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

@@ -175,14 +175,16 @@ public:
cv::Mat * laserScanRaw = 0,
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0);
cv::Mat * obstacleCellsRaw = 0,
cv::Mat * emptyCellsRaw = 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;
cv::Mat * obstacleCellsRaw = 0,
cv::Mat * emptyCellsRaw = 0) const;
const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
@@ -203,6 +205,7 @@ public:
void setOccupancyGrid(
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewPoint);
// remove raw occupancy grids
@@ -211,6 +214,8 @@ public:
const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
float gridCellSize() const {return _cellSize;}
const cv::Point3f & gridViewPoint() const {return _viewPoint;}
@@ -261,8 +266,10 @@ private:
// occupancy grid
cv::Mat _groundCellsCompressed;
cv::Mat _obstacleCellsCompressed;
cv::Mat _emptyCellsCompressed;
cv::Mat _groundCellsRaw;
cv::Mat _obstacleCellsRaw;
cv::Mat _emptyCellsRaw;
float _cellSize;
cv::Point3f _viewPoint;

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

@@ -203,6 +203,7 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNorm
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)

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

@@ -45,8 +45,8 @@ namespace util3d
RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
const cv::Mat & scan, // in /base_link frame
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled = false,
float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
@@ -54,8 +54,8 @@ RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
const cv::Mat & scan, // in /base_link frame
const cv::Point3f & viewpoint, // /base_link -> /base_scan
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled = false,
float scanMaxRange = 0.0f), "Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method.");
@@ -64,8 +64,8 @@ void RTABMAP_EXP occupancy2DFromLaserScan(
const cv::Mat & scanHit, // in /base_link frame
const cv::Mat & scanNoHit, // in /base_link frame
const cv::Point3f & viewpoint, // /base_link -> /base_scan
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled = false,
float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true

4
corelib/src/DBDriver.cpp
View File

@@ -475,17 +475,19 @@ void DBDriver::updateOccupancyGrid(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint)
{
_dbSafeAccessMutex.lock();
//just to make sure the occupancy grids are compressed for convenience
SensorData data;
data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewpoint);
this->updateOccupancyGridQuery(
nodeId,
data.gridGroundCellsCompressed(),
data.gridObstacleCellsCompressed(),
data.gridEmptyCellsCompressed(),
cellSize,
viewpoint);
_dbSafeAccessMutex.unlock();

73
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1220,7 +1220,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
if(occupancyGrid)
{
fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
}
else
{
fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
}
}
query << "SELECT " << fields.str().c_str() << " "
@@ -1557,6 +1564,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
// Occupancy grid
cv::Mat groundCellsCompressed;
cv::Mat obstacleCellsCompressed;
cv::Mat emptyCellsCompressed;
float cellSize = 0.0f;
cv::Point3f viewPoint;
if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
@@ -1579,6 +1587,18 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
}
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
// empty
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize > 0 && data)
{
emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
memcpy((void*)emptyCellsCompressed.data, data, dataSize);
}
}
cellSize = sqlite3_column_double(ppStmt, index++);
viewPoint.x = sqlite3_column_double(ppStmt, index++);
viewPoint.y = sqlite3_column_double(ppStmt, index++);
@@ -1612,11 +1632,11 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
if(occupancyGrid)
{
(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, cellSize, viewPoint);
(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
}
else
{
(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridEmptyCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
}
rc = sqlite3_step(ppStmt); // next result...
}
@@ -3881,6 +3901,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const
{
@@ -3903,6 +3924,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
nodeId,
ground,
obstacles,
empty,
cellSize,
viewpoint);
@@ -4834,7 +4856,11 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.11.10") >= 0)
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else 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(?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
@@ -5046,6 +5072,21 @@ 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.16.0") >= 0)
{
//empty_cells
if(sensorData.gridEmptyCellsCompressed().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.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().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());
@@ -5308,22 +5349,29 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
{
UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
if(uStrNumCmp(_version, "0.16.0") >= 0)
{
return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
}
return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
}
void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const
{
UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d cell=%f viewpoint=(%f,%f,%f)",
UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
nodeId,
ground.cols,
obstacles.cols,
empty.cols,
cellSize,
viewpoint.x,
viewpoint.y,
@@ -5361,6 +5409,21 @@ void DBDriverSqlite3::stepOccupancyGridUpdate(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.16.0") >= 0)
{
//empty_cells
if(empty.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++, empty.data, empty.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++, cellSize);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -89,6 +89,7 @@ private:
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const;
@@ -163,6 +164,7 @@ private:
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const;

6
corelib/src/Memory.cpp
View File

@@ -4043,19 +4043,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
// Occupancy grid map stuff
cv::Mat ground, obstacles;
cv::Mat ground, obstacles, empty;
float cellSize = 0.0f;
cv::Point3f viewPoint(0,0,0);
if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
{
_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
cellSize = _occupancy->getCellSize();
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
UDEBUG("time grid map = %fs", t);
}
s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
// prior
if(!isIntermediateNode)

298
corelib/src/OccupancyGrid.cpp
View File

@@ -32,6 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#ifdef RTABMAP_OCTOMAP
#include <rtabmap/core/OctoMap.h>
#endif
#include <pcl/io/pcd_io.h>
namespace rtabmap {
@@ -64,16 +68,18 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
projRayTracing_(Parameters::defaultGridProjRayTracing()),
rayTracing_(Parameters::defaultGridRayTracing()),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
minMapSize_(Parameters::defaultGridGlobalMinSize()),
erode_(Parameters::defaultGridGlobalEroded()),
footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
updateError_(Parameters::defaultGridGlobalUpdateError()),
xMin_(0.0f),
yMin_(0.0f),
cloudAssembling_(false),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>)
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
{
this->parseParameters(parameters);
}
@@ -117,11 +123,12 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
UASSERT(minMapSize_ >= 0.0f);
@@ -213,8 +220,9 @@ void OccupancyGrid::setCloudAssembling(bool enabled)
void OccupancyGrid::createLocalMap(
const Signature & node,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPoint) const
{
UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
@@ -233,11 +241,13 @@ void OccupancyGrid::createLocalMap(
util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
cv::Mat(),
viewPoint,
ground,
obstacles,
emptyCells,
obstacleCells,
cellSize_,
scan2dUnknownSpaceFilled_,
node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
}
else
{
@@ -300,94 +310,149 @@ void OccupancyGrid::createLocalMap(
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
}
}
createLocalMap(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
}
}
if(projMapFrame_)
void OccupancyGrid::createLocalMap(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const
{
pcl::IndicesPtr indices(new std::vector<int>);
UASSERT_MSG(cloud->size() && cloud->is_dense, uFormat("Use interface with indices if cloud is not dense.").c_str());
createLocalMap(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
}
void OccupancyGrid::createLocalMap(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
const pcl::IndicesPtr & indices,
const Transform & pose,
cv::Mat & groundCells,
cv::Mat & obstacleCells,
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const
{
if(projMapFrame_)
{
//we should rotate viewPoint in /map frame
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
viewPointInOut.x = viewpointRotated.x();
viewPointInOut.y = viewpointRotated.y();
viewPointInOut.z = viewpointRotated.z();
}
if((cloud->is_dense && cloud->size()) ||
(!cloud->is_dense && indices->size()))
{
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = this->segmentCloud<pcl::PointXYZRGB>(
cloud,
indices,
pose,
viewPointInOut,
groundIndices,
obstaclesIndices);
if(!groundIndices->empty() || !obstaclesIndices->empty())
{
//we should rotate viewPoint in /map frame
float roll, pitch, yaw;
node.getPose().getEulerAngles(roll, pitch, yaw);
Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPoint.x, viewPoint.y, viewPoint.z, 0,0,0);
viewPoint.x = viewpointRotated.x();
viewPoint.y = viewpointRotated.y();
viewPoint.z = viewpointRotated.z();
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if((cloud->is_dense && cloud->size()) ||
(!cloud->is_dense && indices->size()))
{
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
cloud = this->segmentCloud<pcl::PointXYZRGB>(
cloud,
indices,
node.getPose(),
viewPoint,
groundIndices,
obstaclesIndices);
if(!groundIndices->empty() || !obstaclesIndices->empty())
if(groundIndices->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloudSegmented, *groundIndices, *groundCloud);
}
if(groundIndices->size())
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloudSegmented, *obstaclesIndices, *obstaclesCloud);
}
if(grid3D_)
{
UDEBUG("");
if(groundIsObstacle_)
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
}
// transform back in base frame
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
if(grid3D_)
if(rayTracing_)
{
UDEBUG("");
if(groundIsObstacle_)
#ifdef RTABMAP_OCTOMAP
if(!groundCloud->empty() || !obstaclesCloud->empty())
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
//create local octomap
OctoMap octomap(cellSize_);
octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
octomap.update(poses);
// transform back in base frame
float roll, pitch, yaw;
node.getPose().getEulerAngles(roll, pitch, yaw);
Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
obstaclesIndices->clear();
groundIndices->clear();
pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
}
}
else
#else
UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
}
#endif
{
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_);
groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
}
if(projRayTracing_)
{
cv::Mat laserScan = obstacles;
cv::Mat laserScanNoHit = ground;
obstacles = cv::Mat();
ground = cv::Mat();
util3d::occupancy2DFromLaserScan(
laserScan,
laserScanNoHit,
viewPoint,
ground,
obstacles,
cellSize_,
false, // don't fill unknown space
0);
}
}
else
{
UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
groundCells,
obstacleCells,
cellSize_);
if(rayTracing_)
{
cv::Mat laserScan = obstacleCells;
cv::Mat laserScanNoHit = groundCells;
obstacleCells = cv::Mat();
groundCells = cv::Mat();
util3d::occupancy2DFromLaserScan(
laserScan,
laserScanNoHit,
viewPointInOut,
emptyCells,
obstacleCells,
cellSize_,
false, // don't fill unknown space
0);
}
}
}
}
UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
}
void OccupancyGrid::clear()
@@ -417,10 +482,11 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
void OccupancyGrid::addToCache(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles)
const cv::Mat & obstacles,
const cv::Mat & empty)
{
UDEBUG("nodeId=%d", nodeId);
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
}
void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
@@ -442,6 +508,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
std::map<int, Transform> transforms;
float updateErrorSqrd = updateError_*updateError_;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
@@ -451,7 +518,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
UASSERT(!iter->second.isNull() && !jter->second.isNull());
Transform t = Transform::getIdentity();
if(iter->second.getDistanceSquared(jter->second) > 0.0001)
if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
{
t = jter->second * iter->second.inverse();
graphOptimized = true;
@@ -487,6 +554,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
bool assembledGroundUpdated = false;
bool assembledObstaclesUpdated = false;
bool assembledEmptyCellsUpdated = false;
if(graphOptimized || graphChanged)
{
@@ -676,22 +744,22 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{
if(uContains(cache_, iter->first))
{
const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
//ground
if(pair.first.cols)
if(pair.first.first.cols)
{
if(pair.first.rows > 1 && pair.first.cols == 1)
if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
}
cv::Mat ground(1, pair.first.cols, CV_32FC2);
cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
{
const float * vi = pair.first.ptr<float>(0,i);
const float * vi = pair.first.first.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
cv::Point3f vt;
if(pair.first.channels() != 2 && pair.first.channels() != 5)
if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
@@ -715,25 +783,67 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(cloudAssembling_)
{
*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first, iter->second, 0, 255, 0);
*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first.first, iter->second, 0, 255, 0);
assembledGroundUpdated = true;
}
}
//obstacles
//empty
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)
cv::Mat ground(1, pair.second.cols, CV_32FC2);
for(int i=0; i<ground.cols; ++i)
{
const float * vi = pair.second.ptr<float>(0,i);
float * vo = ground.ptr<float>(0,i);
cv::Point3f vt;
if(pair.second.channels() != 2 && pair.second.channels() != 5)
{
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));
if(cloudAssembling_)
{
*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 0, 255, 0);
assembledEmptyCellsUpdated = true;
}
}
//obstacles
if(pair.first.second.cols)
{
if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
{
UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
}
cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
for(int i=0; i<obstacles.cols; ++i)
{
const float * vi = pair.first.second.ptr<float>(0,i);
float * vo = obstacles.ptr<float>(0,i);
cv::Point3f vt;
if(pair.first.channels() != 2 && pair.first.channels() != 5)
if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
{
vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
}
@@ -757,7 +867,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
if(cloudAssembling_)
{
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 255, 0, 0);
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.first.second, iter->second, 255, 0, 0);
assembledObstaclesUpdated = true;
}
}
@@ -1165,6 +1275,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
{
assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
}
if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
{
assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
}
}
if(!fullUpdate_ && !cloudAssembling_)
@@ -1174,7 +1288,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
else
{
//clear only negative ids
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
{
if(iter->first < 0)
{

470
corelib/src/OctoMap.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OctoMap.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
@@ -35,24 +36,159 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
//////////////////////////////////////
// RtabmapColorOcTree
//////////////////////////////////////
RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
RtabmapColorOcTreeMemberInit.ensureLinking();
};
RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search (key);
if (n != 0) {
n->setColor(r, g, b);
}
return n;
}
bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
if (!isNodeCollapsible(node))
return false;
// set value to children's values (all assumed equal)
node->copyData(*(getNodeChild(node, 0)));
if (node->isColorSet()) // TODO check
node->setColor(node->getAverageChildColor());
// delete children
for (unsigned int i=0;i<8;i++) {
deleteNodeChild(node, i);
}
delete[] node->children;
node->children = NULL;
return true;
}
bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
// all children must exist, must not have children of
// their own and have the same occupancy probability
if (!nodeChildExists(node, 0))
return false;
const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
if (nodeHasChildren(firstChild))
return false;
for (unsigned int i = 1; i<8; i++) {
// compare nodes only using their occupancy, ignoring color for pruning
if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
return false;
}
return true;
}
RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search(key);
if (n != 0) {
if (n->isColorSet()) {
RtabmapColorOcTreeNode::Color prev_color = n->getColor();
n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
}
else {
n->setColor(r, g, b);
}
}
return n;
}
RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
uint8_t r,
uint8_t g,
uint8_t b) {
RtabmapColorOcTreeNode* n = search (key);
if (n != 0) {
if (n->isColorSet()) {
RtabmapColorOcTreeNode::Color prev_color = n->getColor();
double node_prob = n->getOccupancy();
uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
+ (double) r * (0.99-node_prob));
uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
+ (double) g * (0.99-node_prob));
uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
+ (double) b * (0.99-node_prob));
n->setColor(new_r, new_g, new_b);
}
else {
n->setColor(r, g, b);
}
}
return n;
}
void RtabmapColorOcTree::updateInnerOccupancy() {
this->updateInnerOccupancyRecurs(this->root, 0);
}
void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
// only recurse and update for inner nodes:
if (nodeHasChildren(node)){
// return early for last level:
if (depth < this->tree_depth){
for (unsigned int i=0; i<8; i++) {
if (nodeChildExists(node, i)) {
updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
}
}
}
node->updateOccupancyChildren();
node->updateColorChildren();
}
}
//////////////////////////////////////
// OctoMap
//////////////////////////////////////
OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
hasColor_(false),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate())
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
updateError_(Parameters::defaultGridGlobalUpdateError())
{
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
UASSERT(cellSize>0.0f);
octree_ = new octomap::ColorOcTree(cellSize);
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
octree_ = new RtabmapColorOcTree(cellSize);
octree_->setOccupancyThres(occupancyThr);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
}
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(cellSize)),
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
octree_(new RtabmapColorOcTree(cellSize)),
hasColor_(false),
fullUpdate_(fullUpdate)
fullUpdate_(fullUpdate),
updateError_(updateError)
{
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
octree_->setOccupancyThres(occupancyThr);
UASSERT(cellSize>0.0f);
}
@@ -66,13 +202,14 @@ OctoMap::~OctoMap()
void OctoMap::clear()
{
octree_->clear();
occupiedCells_.clear();
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false;
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
}
void OctoMap::addToCache(int nodeId,
@@ -87,12 +224,14 @@ void OctoMap::addToCache(int nodeId,
void OctoMap::addToCache(int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
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));
UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
UDEBUG("nodeId=%d", nodeId);
uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
}
@@ -105,6 +244,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
std::map<int, Transform> transforms;
std::map<int, Transform> updatedAddedNodes;
float updateErrorSqrd = updateError_*updateError_;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
@@ -113,7 +253,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
graphChanged = false;
UASSERT(!iter->second.isNull() && !jter->second.isNull());
Transform t = Transform::getIdentity();
if(iter->second.getDistanceSquared(jter->second) > 0.0001)
if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
{
t = jter->second * iter->second.inverse();
graphOptimized = true;
@@ -137,64 +277,99 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UINFO("Graph optimized!");
}
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
if(fullUpdate_ || graphChanged)
{
// clear all but keep cache
octree_->clear();
occupiedCells_.clear();
addedNodes_.clear();
keyRay_ = octomap::KeyRay();
hasColor_ = false;
}
else
{
octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
int copied=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
int count=0;
UTimer t;
for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
{
std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
if(jter != transforms.end())
RtabmapColorOcTreeNode & nOld = *it;
if(nOld.getNodeRefId() > 0)
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
UASSERT(pter != addedNodes_.end());
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
if(jter != transforms.end())
{
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
octomap::point3d pt;
std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
UASSERT(pter != addedNodes_.end());
if(nOld.getOccupancyType() > 0)
{
++copied;
uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
pt = nOld.getPointRef();
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
pt = octree_->keyToCoord(it.getKey());
}
cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
cvPt = util3d::transformPoint(cvPt, jter->second);
octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(ptTransformed, key))
{
RtabmapColorOcTreeNode * n = newOcTree->search(key);
if(n)
{
if(n->getNodeRefId() > nOld.getNodeRefId())
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
{
// empty cells cannot overwrite ground/obstacle cells
continue;
}
}
RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
if(nNew)
{
++copied;
updateMinMax(ptTransformed);
nNew->setNodeRefId(nOld.getNodeRefId());
if(nOld.getOccupancyType() > 0)
{
nNew->setPointRef(pt);
}
nNew->setOccupancyType(nOld.getOccupancyType());
nNew->setColor(nOld.getColor());
}
else
{
UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
{
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
}
}
else
else if(jter == transforms.end())
{
UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
// Note: normal if old nodes were transfered to LTM
//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
// Note: normal if old nodes were transfered to LTM
//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
delete octree_;
octree_ = newOcTree;
occupiedCells_ = newOccupiedCells;
//update added poses
addedNodes_ = updatedAddedNodes;
@@ -235,7 +410,7 @@ 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;
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
std::map<int, cv::Point3f>::iterator viewPointIter;
cloudIter = cacheClouds_.find(iter->first);
occupancyIter = cache_.find(iter->first);
@@ -248,6 +423,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
updateMinMax(sensorOrigin);
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -255,10 +432,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
bool computeRays = occupancyIter == cache_.end() || occupancyIter->second.second.empty();
// instead of direct scan insertion, compute update to filter ground:
octomap::KeySet free_cells, occupied_cells, ground_cells;
octomap::KeySet free_cells;
// insert ground points only as free:
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
Eigen::Affine3f t = iter->second.toEigen3f();
for (unsigned int i=0; i<maxGroundPts; ++i)
@@ -266,7 +445,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
pt = util3d::laserScanToPointRGB(occupancyIter->second.first.first, i);
pt = pcl::transformPoint(pt, t);
}
else
@@ -277,7 +456,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if ((iter->first < 0 || iter->first>lastId) &&
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -288,17 +468,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
ground_cells.insert(key);
updateMinMax(point);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -308,26 +488,24 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
}
}
}
UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
// all other points: free on ray, occupied on endpoint:
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
for (unsigned int i=0; i<maxObstaclePts; ++i)
{
pcl::PointXYZRGB pt;
if(occupancyIter != cache_.end())
{
pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
pt = util3d::laserScanToPointRGB(occupancyIter->second.first.second, i);
pt = pcl::transformPoint(pt, t);
}
else
@@ -338,7 +516,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if ((iter->first < 0 || iter->first>lastId) &&
if (computeRays &&
(iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -349,17 +528,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
occupied_cells.insert(key);
updateMinMax(point);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
if(n)
{
if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -369,32 +548,60 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
if(iter->first > 0)
{
uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
}
else
{
occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
n->setNodeRefId(iter->first);
n->setPointRef(point);
}
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
}
}
}
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
// mark free cells only if not seen occupied in this cloud
for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
{
if (occupied_cells.find(*it) == occupied_cells.end() &&
ground_cells.find(*it) == ground_cells.end())
RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{
octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
if(n)
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
n->setNodeRefId(iter->first);
}
}
// all empty cells
if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
{
unsigned int maxEmptyPts = occupancyIter->second.second.cols;
UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
for (unsigned int i=0; i<maxEmptyPts; ++i)
{
pcl::PointXYZ pt;
pt = util3d::laserScanToPoint(occupancyIter->second.second, i);
pt = pcl::transformPoint(pt, t);
octomap::point3d point(pt.x, pt.y, pt.z);
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
gter = occupiedCells_.find(n);
if(gter != occupiedCells_.end() && gter->second.isObstacle_)
updateMinMax(point);
if(iter->first >0 && iter->first<lastId)
{
occupiedCells_.erase(gter);
RtabmapColorOcTreeNode * n = octree_->search(key);
if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
n->setNodeRefId(iter->first);
}
}
}
@@ -415,6 +622,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("Did not find %d in cache", iter->first);
}
}
if(!fullUpdate_)
{
cache_.clear();
@@ -423,7 +631,35 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
}
void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
void OctoMap::updateMinMax(const octomap::point3d & point)
{
if(point.x() < minValues_[0])
{
minValues_[0] = point.x();
}
if(point.y() < minValues_[1])
{
minValues_[1] = point.y();
}
if(point.z() < minValues_[2])
{
minValues_[2] = point.z();
}
if(point.x() > maxValues_[0])
{
maxValues_[0] = point.x();
}
if(point.y() > maxValues_[1])
{
maxValues_[1] = point.y();
}
if(point.z() > maxValues_[2])
{
maxValues_[2] = point.z();
}
}
void OctoMap::HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
{
int i;
float f, p, q, t;
@@ -475,7 +711,9 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
unsigned int treeDepth,
std::vector<int> * obstacleIndices,
std::vector<int> * emptyIndices) const
std::vector<int> * emptyIndices,
std::vector<int> * groundIndices,
bool originalRefPoints) const
{
UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -490,22 +728,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
emptyIndices->resize(octree_->size());
}
if(groundIndices)
{
groundIndices->resize(octree_->size());
}
if(treeDepth == 0)
{
treeDepth = octree_->getTreeDepth();
}
double minX, minY, minZ, maxX, maxY, maxZ;
octree_->getMetricMin(minX, minY, minZ);
octree_->getMetricMax(maxX, maxY, maxZ);
double minZ = minValues_[2];
double maxZ = maxValues_[2];
bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
int oi=0;
int si=0;
int ei=0;
int gi=0;
for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
if(octree_->isNodeOccupied(*it) && (obstacleIndices || emptyIndices == 0))
if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || addAllPoints))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
@@ -522,25 +766,45 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
(*cloud)[oi].g = g*255.0f;
(*cloud)[oi].b = b*255.0f;
}
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
(*cloud)[oi].z = pt.z();
if(originalRefPoints && it->getOccupancyType() > 0)
{
const octomap::point3d & p = it->getPointRef();
(*cloud)[oi].x = p.x();
(*cloud)[oi].y = p.y();
(*cloud)[oi].z = p.z();
}
else
{
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
(*cloud)[oi].z = pt.z();
}
if(obstacleIndices)
{
obstacleIndices->at(si++) = oi;
}
++oi;
}
else if(emptyIndices || obstacleIndices == 0)
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || groundIndices != 0 || addAllPoints))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
(*cloud)[oi].z = pt.z();
if(emptyIndices)
if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
{
emptyIndices->at(gi++) = oi;
if(groundIndices)
{
groundIndices->at(gi++) = oi;
}
}
else if(emptyIndices)
{
emptyIndices->at(ei++) = oi;
}
++oi;
}
@@ -550,10 +814,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
if(obstacleIndices)
{
obstacleIndices->resize(si);
UDEBUG("obstacle=%d", si);
}
if(emptyIndices)
{
emptyIndices->resize(gi);
emptyIndices->resize(ei);
UDEBUG("empty=%d", ei);
}
if(groundIndices)
{
groundIndices->resize(gi);
UDEBUG("ground=%d", gi);
}
UDEBUG("");
@@ -569,6 +840,7 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
}
gridCellSize = octree_->getNodeSize(treeDepth);
float halfCellSize = gridCellSize/2.0f;
pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -577,16 +849,16 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
obstacles->resize(octree_->size());
int gi=0;
int oi=0;
for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->isNodeOccupied(*it))
{
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
}
else
{
(*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
(*ground)[gi++] = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
}
}
obstacles->resize(oi);
@@ -594,11 +866,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
if(obstacles->size())
{
obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
obstacles = util3d::voxelize(obstacles, halfCellSize);
}
if(ground->size())
{
ground = util3d::voxelize(ground, gridCellSize/2.0f);
ground = util3d::voxelize(ground, halfCellSize);
}
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);

4
corelib/src/Parameters.cpp
View File

@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
if(removedParameters_.empty())
{
// removed parameters
// 0.16.0
removedParameters_.insert(std::make_pair("Grid/ProjRayTracing", std::make_pair(true, Parameters::kGridRayTracing())));
// 0.15.1
removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("Reg/VarianceNormalized", std::make_pair(false, "")));

2
corelib/src/Rtabmap.cpp
View File

@@ -2128,7 +2128,7 @@ bool Rtabmap::process(
}
else
{
UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
}
}
}

70
corelib/src/SensorData.cpp
View File

@@ -476,12 +476,14 @@ void SensorData::setUserData(const cv::Mat & userData)
void SensorData::setOccupancyGrid(
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewPoint)
{
UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
(!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
{
UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
return;
@@ -491,9 +493,12 @@ void SensorData::setOccupancyGrid(
_groundCellsCompressed = cv::Mat();
_obstacleCellsRaw = cv::Mat();
_obstacleCellsCompressed = cv::Mat();
_emptyCellsRaw = cv::Mat();
_emptyCellsCompressed = cv::Mat();
CompressionThread ctGround(ground);
CompressionThread ctObstacles(obstacles);
CompressionThread ctEmpty(empty);
if(!ground.empty())
{
@@ -521,8 +526,22 @@ void SensorData::setOccupancyGrid(
_obstacleCellsCompressed = obstacles;
}
}
if(!empty.empty())
{
if(empty.type() == CV_32FC2 || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(5) || empty.type() == CV_32FC(6) || empty.type() == CV_32FC(7))
{
_emptyCellsRaw = empty;
ctEmpty.start();
}
else if(empty.type() == CV_8UC1)
{
UASSERT(empty.type() == CV_8UC1); // Bytes
_emptyCellsCompressed = empty;
}
}
ctGround.join();
ctObstacles.join();
ctEmpty.join();
if(!_groundCellsRaw.empty())
{
_groundCellsCompressed = ctGround.getCompressedData();
@@ -531,6 +550,10 @@ void SensorData::setOccupancyGrid(
{
_obstacleCellsCompressed = ctObstacles.getCompressedData();
}
if(!_emptyCellsRaw.empty())
{
_emptyCellsCompressed = ctEmpty.getCompressedData();
}
_cellSize = cellSize;
_viewPoint = viewPoint;
@@ -538,13 +561,14 @@ void SensorData::setOccupancyGrid(
void SensorData::uncompressData()
{
cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF, tmpG;
uncompressData(_imageCompressed.empty()?0:&tmpA,
_depthOrRightCompressed.empty()?0:&tmpB,
_laserScanCompressed.empty()?0:&tmpC,
_userDataCompressed.empty()?0:&tmpD,
_groundCellsCompressed.empty()?0:&tmpE,
_obstacleCellsCompressed.empty()?0:&tmpF);
_obstacleCellsCompressed.empty()?0:&tmpF,
_emptyCellsCompressed.empty()?0:&tmpG);
}
void SensorData::uncompressData(
@@ -553,15 +577,17 @@ void SensorData::uncompressData(
cv::Mat * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw)
cv::Mat * obstacleCellsRaw,
cv::Mat * emptyCellsRaw)
{
UDEBUG("%d data(%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0);
UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
if(imageRaw == 0 &&
depthRaw == 0 &&
laserScanRaw == 0 &&
userDataRaw == 0 &&
groundCellsRaw == 0 &&
obstacleCellsRaw == 0)
obstacleCellsRaw == 0 &&
emptyCellsRaw == 0)
{
return;
}
@@ -571,7 +597,8 @@ void SensorData::uncompressData(
laserScanRaw,
userDataRaw,
groundCellsRaw,
obstacleCellsRaw);
obstacleCellsRaw,
emptyCellsRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
@@ -609,6 +636,10 @@ void SensorData::uncompressData(
{
_obstacleCellsRaw = *obstacleCellsRaw;
}
if(emptyCellsRaw && !emptyCellsRaw->empty() && _emptyCellsRaw.empty())
{
_emptyCellsRaw = *emptyCellsRaw;
}
}
void SensorData::uncompressDataConst(
@@ -617,7 +648,8 @@ void SensorData::uncompressDataConst(
cv::Mat * laserScanRaw,
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw) const
cv::Mat * obstacleCellsRaw,
cv::Mat * emptyCellsRaw) const
{
if(imageRaw)
{
@@ -643,12 +675,17 @@ void SensorData::uncompressDataConst(
{
*obstacleCellsRaw = _obstacleCellsRaw;
}
if(emptyCellsRaw)
{
*emptyCellsRaw = _emptyCellsRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()) ||
(userDataRaw && userDataRaw->empty()) ||
(groundCellsRaw && groundCellsRaw->empty()) ||
(obstacleCellsRaw && obstacleCellsRaw->empty()))
(obstacleCellsRaw && obstacleCellsRaw->empty()) ||
(emptyCellsRaw && emptyCellsRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
@@ -656,6 +693,7 @@ void SensorData::uncompressDataConst(
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
rtabmap::CompressionThread ctEmptyCells(_emptyCellsCompressed, false);
if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
{
UASSERT(_imageCompressed.type() == CV_8UC1);
@@ -686,12 +724,18 @@ void SensorData::uncompressDataConst(
UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
ctObstacleCells.start();
}
if(emptyCellsRaw && emptyCellsRaw->empty() && !_emptyCellsCompressed.empty())
{
UASSERT(_emptyCellsCompressed.type() == CV_8UC1);
ctEmptyCells.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
ctUserData.join();
ctGroundCells.join();
ctObstacleCells.join();
ctEmptyCells.join();
if(imageRaw && imageRaw->empty())
{
@@ -763,6 +807,10 @@ void SensorData::uncompressDataConst(
{
*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
}
if(emptyCellsRaw && emptyCellsRaw->empty())
{
*emptyCellsRaw = ctEmptyCells.getUncompressedData();
}
}
}
@@ -789,6 +837,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
_keypoints.size() * sizeof(float) * 7 +
_keypoints3D.size() * sizeof(float)*3 +
_descriptors.total()*_descriptors.elemSize();

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

@@ -39,6 +39,7 @@ CREATE TABLE Data (
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)
empty_cells BLOB, -- compressed data (occupancy grid)
cell_size FLOAT,
view_point_x FLOAT,
view_point_y FLOAT,
@@ -114,6 +115,9 @@ CREATE TABLE Admin (
opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
opt_tex_materials BLOB, -- compressed image
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
time_enter DATE
);

61
corelib/src/util3d.cpp
View File

@@ -1358,27 +1358,60 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(4));
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
{
cv::Mat laserScan;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
if(indices.get())
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
for(unsigned int i=0; i<indices->size(); ++i)
{
pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
float * ptr = laserScan.ptr<float>(0, i);
int index = indices->at(i);
if(!nullTransform)
{
pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(index).x;
ptr[1] = cloud.at(index).y;
ptr[2] = cloud.at(index).z;
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
}
else
}
else
{
laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
for(unsigned int i=0; i<cloud.size(); ++i)
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
return laserScan;
}

36
corelib/src/util3d_mapping.cpp
View File

@@ -48,8 +48,8 @@ namespace util3d
void occupancy2DFromLaserScan(
const cv::Mat & scan,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
@@ -59,8 +59,8 @@ void occupancy2DFromLaserScan(
scan,
cv::Mat(),
viewpoint,
ground,
obstacles,
empty,
occupied,
cellSize,
unknownSpaceFilled,
scanMaxRange);
@@ -69,21 +69,21 @@ void occupancy2DFromLaserScan(
void occupancy2DFromLaserScan(
const cv::Mat & scan,
const cv::Point3f & viewpoint,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
{
occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, ground, obstacles, cellSize, unknownSpaceFilled, scanMaxRange);
occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, empty, occupied, cellSize, unknownSpaceFilled, scanMaxRange);
}
void occupancy2DFromLaserScan(
const cv::Mat & scanHit,
const cv::Mat & scanNoHit,
const cv::Point3f & viewpoint,
cv::Mat & ground,
cv::Mat & obstacles,
cv::Mat & empty,
cv::Mat & occupied,
float cellSize,
bool unknownSpaceFilled,
float scanMaxRange)
@@ -105,27 +105,27 @@ void occupancy2DFromLaserScan(
float xMin, yMin;
cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
// find ground cells
std::list<int> groundIndices;
// find empty cells
std::list<int> emptyIndices;
for(unsigned int i=0; i< map8S.total(); ++i)
{
if(map8S.data[i] == 0)
{
groundIndices.push_back(i);
emptyIndices.push_back(i);
}
}
// Convert to position matrices, get points to each center of the cells
ground = cv::Mat();
if(groundIndices.size())
empty = cv::Mat();
if(emptyIndices.size())
{
ground = cv::Mat(1, (int)groundIndices.size(), CV_32FC2);
empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
int i=0;
for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
{
int y = *iter / map8S.cols;
int x = *iter - y*map8S.cols;
cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
ptr[i][0] = (float(x))*cellSize + xMin;
ptr[i][1] = (float(y))*cellSize + yMin;
++i;
@@ -133,7 +133,7 @@ void occupancy2DFromLaserScan(
}
// copy directly obstacles precise positions
obstacles = scanHit.clone();
occupied = scanHit.clone();
}
/**