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

Fixing various issues detected by rtabmap_slam tests (#1773)

Browse Source 
* Fixing getNodeData missing compressed grids

* Fixing roundtrip laserScan <-> Pointcloud2 on all formats.

* Keep already compressed user_data and laser_scan if possible

* fixed double compression of user_data

* reorder headers

* removed dead function declaration

* expand test coverage
This commit is contained in:
matlabbe
2026-09-26 21:07:59 -07:00
committed by GitHub
parent 16fb2f0541
commit 66c72be7db
12 changed files with 396 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBDriver.cpp
+4 -1
View File
@@ -707,7 +707,10 @@ void DBDriver::getNodeData(
((!images || !s->sensorData().imageCompressed().empty()) &&
(!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
(!userData || !s->sensorData().userDataCompressed().empty()) &&
(!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
(!occupancyGrid ||
!s->sensorData().gridGroundCellsCompressed().empty() ||
!s->sensorData().gridObstacleCellsCompressed().empty() ||
!s->sensorData().gridEmptyCellsCompressed().empty()))))
{
data = (SensorData)s->sensorData();
if(!images)
corelib/src/Memory.cpp
+24 -13
View File
@@ -4795,7 +4795,10 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
((!images || !s->sensorData().imageCompressed().empty()) &&
(!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
(!userData || !s->sensorData().userDataCompressed().empty()) &&
(!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
(!occupancyGrid ||
!s->sensorData().gridGroundCellsCompressed().empty() ||
!s->sensorData().gridObstacleCellsCompressed().empty() ||
!s->sensorData().gridEmptyCellsCompressed().empty()))))
{
r = s->sensorData();
if(!images)
@@ -6669,6 +6672,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool reuseCompressedDepthConfidence =
depthConfidence.data == data.depthConfidenceRaw().data &&
!data.depthConfidenceCompressed().empty();
bool reuseCompressedUserData = !data.userDataCompressed().empty();
bool reuseCompressedScan =
laserScan.data().data == data.laserScanRaw().data().data &&
!data.laserScanCompressed().isEmpty();
cv::Mat compressedImage;
cv::Mat compressedDepth;
@@ -6694,11 +6701,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
ctDepthConfidence.start();
}
if(!laserScan.isEmpty())
if(!laserScan.isEmpty() && !reuseCompressedScan)
{
ctLaserScan.start();
}
if(!data.userDataRaw().empty())
if(!data.userDataRaw().empty() && !reuseCompressedUserData)
{
ctUserData.start();
}
@@ -6711,16 +6718,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedDepthConfidence = ctDepthConfidence.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
compressedUserData = ctUserData.getCompressedData();
compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
compressedUserData = reuseCompressedUserData?data.userDataCompressed():ctUserData.getCompressedData();
}
else
{
compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
compressedDepthConfidence = reuseCompressedDepthConfidence?cv::Mat():compressData2(depthConfidence);
compressedScan = compressData2(laserScan.data());
compressedUserData = compressData2(data.userDataRaw());
compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
}
s = new Signature(id,
@@ -6784,28 +6791,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// just compress user data and laser scan (scans can be used for local scan matching)
cv::Mat compressedScan;
cv::Mat compressedUserData;
bool reuseCompressedUserData = !data.userDataCompressed().empty();
bool reuseCompressedScan =
laserScan.data().data == data.laserScanRaw().data().data &&
!data.laserScanCompressed().isEmpty();
if(_compressionParallelized)
{
rtabmap::CompressionThread ctUserData(data.userDataRaw());
rtabmap::CompressionThread ctLaserScan(laserScan.data());
if(!data.userDataRaw().empty() && !isIntermediateNode)
if(!data.userDataRaw().empty() && !isIntermediateNode && !reuseCompressedUserData)
{
ctUserData.start();
}
if(!laserScan.isEmpty() && !isIntermediateNode)
if(!laserScan.isEmpty() && !isIntermediateNode && !reuseCompressedScan)
{
ctLaserScan.start();
}
ctUserData.join();
ctLaserScan.join();
compressedScan = ctLaserScan.getCompressedData();
compressedUserData = ctUserData.getCompressedData();
compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
compressedUserData = reuseCompressedUserData && !isIntermediateNode?data.userDataCompressed():ctUserData.getCompressedData();
}
else
{
compressedScan = compressData2(laserScan.data());
compressedUserData = compressData2(data.userDataRaw());
compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
}
s = new Signature(id,
corelib/src/Rtabmap.cpp
+5
View File
@@ -5920,6 +5920,11 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
s.sensorData().setGlobalDescriptors(globalDescriptors);
}
}
if(!withGlobalDescriptors)
{
// Node data taken from memory comes with its global descriptors.
s.sensorData().clearGlobalDescriptors();
}
if(velocity.size()==6)
{
s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
corelib/src/SensorData.cpp
+1 -1
View File
@@ -568,7 +568,7 @@ void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
else
{
_userDataRaw = userData;
if(!userData.empty())
if(!userData.empty() && _userDataCompressed.empty())
{
_userDataCompressed = compressData2(userData);
}
corelib/src/util3d.cpp
+50 -1
View File
@@ -2328,10 +2328,59 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
{
pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
}
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
{
pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
}
else if(laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
{
// PCL has no point type with time (and ring): append them to the XYZI fields, with
// the types laserScanFromPointCloud() reads back (time FLOAT32, ring UINT16).
pcl::PCLPointCloud2 xyzi;
pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), xyzi);
const bool hasRing = laserScan.format() == LaserScan::kXYZIRT;
cloud->header = xyzi.header;
cloud->height = xyzi.height;
cloud->width = xyzi.width;
cloud->is_bigendian = xyzi.is_bigendian;
cloud->is_dense = xyzi.is_dense;
cloud->fields = xyzi.fields;
pcl::PCLPointField time;
time.name = "time";
time.offset = xyzi.point_step;
time.datatype = pcl::PCLPointField::FLOAT32;
time.count = 1;
cloud->fields.push_back(time);
cloud->point_step = xyzi.point_step + 4;
pcl::PCLPointField ring;
if(hasRing)
{
ring.name = "ring";
ring.offset = cloud->point_step;
ring.datatype = pcl::PCLPointField::UINT16;
ring.count = 1;
cloud->fields.push_back(ring);
cloud->point_step += 4; // keep points 4-byte aligned
}
cloud->row_step = cloud->point_step * cloud->width;
cloud->data.resize(size_t(cloud->row_step) * cloud->height, 0);
const int cols = laserScan.data().cols;
const size_t points = size_t(cloud->width) * cloud->height;
for(size_t i=0; i<points; ++i)
{
unsigned char * dst = &cloud->data[i * cloud->point_step];
memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step);
const float * src = laserScan.data().ptr<float>(int(i) / cols, int(i) % cols);
memcpy(dst + time.offset, src + laserScan.getTimeOffset(), sizeof(float));
if(hasRing)
{
const std::uint16_t r = (std::uint16_t)src[laserScan.getRingOffset()];
memcpy(dst + ring.offset, &r, sizeof(r));
}
}
}
else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
{
pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);