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

DBViewer: Added option to crop empty space from local grid and scans after 2d occupancy grid is manually edited

Browse Source
This commit is contained in:
matlabbe
2020-02-13 17:32:39 -05:00
parent 79b480da4b
commit 5fef44ec2b
11 changed files with 558 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/DBDriver.cpp
View File

@@ -511,6 +511,15 @@ void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
_dbSafeAccessMutex.unlock();
}
void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
{
_dbSafeAccessMutex.lock();
this->updateLaserScanQuery(
nodeId,
scan);
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
{
_dbSafeAccessMutex.lock();

173
corelib/src/DBDriverSqlite3.cpp
View File

@@ -4408,12 +4408,12 @@ void DBDriverSqlite3::updateLinkQuery(const Link & link) const
}
void DBDriverSqlite3::updateOccupancyGridQuery(
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const
int nodeId,
const cv::Mat & ground,
const cv::Mat & obstacles,
const cv::Mat & empty,
float cellSize,
const cv::Point3f & viewpoint) const
{
UDEBUG("");
if(_ppDb)
@@ -4447,8 +4447,8 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
}
void DBDriverSqlite3::updateDepthImageQuery(
int nodeId,
const cv::Mat & image) const
int nodeId,
const cv::Mat & image) const
{
UDEBUG("");
if(_ppDb)
@@ -4477,6 +4477,37 @@ void DBDriverSqlite3::updateDepthImageQuery(
}
}
void DBDriverSqlite3::updateLaserScanQuery(
int nodeId,
const LaserScan & scan) const
{
UDEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
// Create query
std::string query = queryStepScanUpdate();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
// Save depth
stepScanUpdate(ppStmt,
nodeId,
scan);
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
}
void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
{
UDEBUG("Ref ID = %d", statistics.refImageId());
@@ -5622,6 +5653,132 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepScanUpdate() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
return "UPDATE Data SET scan_info=?, scan=? WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.10.7") >= 0)
{
return "UPDATE Data SET scan_max_pts=?, scan_max_range=?, scan=? WHERE id=?;";
}
else
{
return "UPDATE Data SET scan_max_pts=? scan=? WHERE id=?;";
}
}
void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & scan) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
std::vector<float> scanInfo;
if(uStrNumCmp(_version, "0.11.10") >= 0)
{
if(scan.maxPoints() > 0 ||
scan.rangeMax() > 0 ||
(uStrNumCmp(_version, "0.16.1")>=0 && scan.format() != LaserScan::kUnknown) ||
(!scan.localTransform().isNull() && !scan.localTransform().isIdentity()))
{
if(uStrNumCmp(_version, "0.16.1") >=0)
{
if(uStrNumCmp(_version, "0.18.0") >=0)
{
scanInfo.resize(7 + Transform().size());
scanInfo[0] = scan.format();
scanInfo[1] = scan.rangeMin();
scanInfo[2] = scan.rangeMax();
scanInfo[3] = scan.angleMin();
scanInfo[4] = scan.angleMax();
scanInfo[5] = scan.angleIncrement();
scanInfo[6] = scan.maxPoints(); // only for backward compatibility
const Transform & localTransform = scan.localTransform();
memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
else
{
scanInfo.resize(3 + Transform().size());
scanInfo[0] = scan.maxPoints();
scanInfo[1] = scan.rangeMax();
scanInfo[2] = scan.format();
const Transform & localTransform = scan.localTransform();
memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
}
}
else
{
scanInfo.resize(2 + Transform().size());
scanInfo[0] = scan.maxPoints();
scanInfo[1] = scan.rangeMax();
const Transform & localTransform = scan.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++, scan.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++, scan.rangeMax());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
// scan
cv::Mat scanCompressed;
if(scan.isCompressed())
{
scanCompressed = scan.data();
}
else
{
scanCompressed = compressData2(scan.data());
}
if(!scanCompressed.empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, scanCompressed.data, scanCompressed.total(), 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());
//id
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
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());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);

42
corelib/src/util3d.cpp
View File

@@ -1263,7 +1263,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
return scan;
}
LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs)
LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs, bool is2D, const Transform & transform)
{
if(cloud.data.empty())
{
@@ -1284,7 +1284,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
fieldStates[1] = 1;
fieldOffsets[1] = cloud.fields[i].offset;
}
else if(cloud.fields[i].name.compare("z") == 0)
else if(cloud.fields[i].name.compare("z") == 0 && !is2D)
{
fieldStates[2] = 1;
fieldOffsets[2] = cloud.fields[i].offset;
@@ -1332,15 +1332,23 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
LaserScan::Format format;
int outputNormalOffset = 0;
if(is3D)
{
if(hasNormals && hasIntensity)
{
format = LaserScan::kXYZINormal;
outputNormalOffset = 4;
}
else if(hasNormals && !hasIntensity && !hasRGB)
{
format = LaserScan::kXYZNormal;
outputNormalOffset = 3;
}
else if(hasNormals && hasRGB)
{
format = LaserScan::kXYZRGBNormal;
outputNormalOffset = 4;
}
else if(!hasNormals && hasIntensity)
{
@@ -1360,6 +1368,12 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
if(hasNormals && hasIntensity)
{
format = LaserScan::kXYINormal;
outputNormalOffset = 3;
}
else if(hasNormals && !hasIntensity)
{
format = LaserScan::kXYNormal;
outputNormalOffset = 2;
}
else if(!hasNormals && hasIntensity)
{
@@ -1374,6 +1388,12 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
bool transformValid = !transform.isNull() && !transform.isIdentity();
Transform transformRot;
if(transformValid)
{
transformRot = transform.rotation();
}
int oi=0;
for (uint32_t row = 0; row < cloud.height; ++row)
{
@@ -1479,6 +1499,24 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
if(!filterNaNs || valid)
{
if(valid && transformValid)
{
cv::Point3f pt = util3d::transformPoint(cv::Point3f(ptr[0], ptr[1], is3D?ptr[3]:0), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
if(is3D)
{
ptr[2] = pt.z;
}
if(hasNormals)
{
pt = util3d::transformPoint(cv::Point3f(ptr[outputNormalOffset], ptr[outputNormalOffset+1], ptr[outputNormalOffset+2]), transformRot);
ptr[outputNormalOffset] = pt.x;
ptr[outputNormalOffset+1] = pt.y;
ptr[outputNormalOffset+2] = pt.z;
}
}
++oi;
}
}

26
corelib/src/util3d_filtering.cpp
View File

@@ -58,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
#include <pcl/filters/impl/extract_indices.hpp>
PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal))
PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal));
PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal));
PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal));
PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal));
#endif
@@ -650,6 +650,24 @@ pcl::IndicesPtr cropBoxImpl(
return output;
}
pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
pcl::IndicesPtr output(new std::vector<int>);
pcl::CropBox<pcl::PCLPointCloud2> filter;
filter.setNegative(negative);
filter.setMin(min);
filter.setMax(max);
if(!transform.isNull() && !transform.isIdentity())
{
filter.setTransform(transform.toEigen3f());
}
filter.setInputCloud(cloud);
filter.setIndices(indices);
filter.filter(*output);
return output;
}
pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
{
return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);