i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 21:10:19 +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-31 12:43:53 -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
corelib/src/CMakeLists.txt
+1 -1
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
corelib/src/CameraModel.cpp
+30
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)
corelib/src/CameraRGB.cpp
+4 -3
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;
}
corelib/src/CameraStereo.cpp
+1 -1
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());
}
}
corelib/src/CameraThread.cpp
+11 -18
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);
}
corelib/src/DBDriver.cpp
+10 -5
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();
}
corelib/src/DBDriverSqlite3.cpp
+340 -164
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());
corelib/src/DBDriverSqlite3.h
+1 -1
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;
corelib/src/Features2d.cpp
+2 -68
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);
}
/////////////////////
corelib/src/Memory.cpp
+36 -65
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;
}
corelib/src/Occupancy.cpp
-201
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_);
}
}
}
}
}
corelib/src/OccupancyGrid.cpp
+979
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());
}
}
corelib/src/OctoMap.cpp
+69 -13
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;
corelib/src/OdometryF2F.cpp
+4 -4
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());
}
}
}
corelib/src/OdometryF2M.cpp
+4 -4
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
corelib/src/OdometryThread.cpp
+1
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));
}
}
corelib/src/Parameters.cpp
+21 -6
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)
{
corelib/src/RegistrationIcp.cpp
+12 -10
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(
corelib/src/Rtabmap.cpp
+10 -3
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,
corelib/src/SensorData.cpp
+170 -45
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
corelib/src/Signature.cpp
+1 -4
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)
{
corelib/src/StereoCameraModel.cpp
+6
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);
corelib/src/resources/DatabaseSchema.sql.in
+11 -5
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)
corelib/src/util2d.cpp
+86
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);
corelib/src/util3d.cpp
+232 -51
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,
corelib/src/util3d_mapping.cpp
+73 -48
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;
}
corelib/src/util3d_transforms.cpp
+12 -1
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;