i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 14:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improved/optimized Rtabmap timing performance for large datasets

Browse Source
This commit is contained in:
matlabbe
2018-11-09 17:28:55 -05:00
parent 3bb874825f
commit 8333677dc6
13 changed files with 588 additions and 467 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/NoEventsExample/MapBuilder.h
+2 -2
View File
@@ -168,9 +168,9 @@ public:
}
cloudViewer_->setCloudVisibility(cloudName, true);
}
else if(uContains(stats.getSignatures(), iter->first))
else if(iter->first == stats.getLastSignatureData().id())
{
Signature s = stats.getSignatures().at(iter->first);
Signature s = stats.getLastSignatureData();
s.sensorData().uncompressData(); // make sure data is uncompressed
// Add the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
examples/RGBDMapping/MapBuilder.h
+8 -17
View File
@@ -192,9 +192,9 @@ protected Q_SLOTS:
}
cloudViewer_->setCloudVisibility(cloudName, true);
}
else if(uContains(stats.getSignatures(), iter->first))
else if(iter->first == stats.getLastSignatureData().id())
{
Signature s = stats.getSignatures().at(iter->first);
Signature s = stats.getLastSignatureData();
s.sensorData().uncompressData(); // make sure data is uncompressed
// Add the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
@@ -246,25 +246,16 @@ protected Q_SLOTS:
//============================
// Update/add occupancy grid (when RGBD/CreateOccupancyGrid is true)
//============================
for(std::map<int, Transform>::const_reverse_iterator iter = stats.poses().rbegin(); iter!=stats.poses().rend(); ++iter)
if(grid_.addedNodes().find(stats.getLastSignatureData().id()) == grid_.addedNodes().end())
{
int id = iter->first;
if(grid_.addedNodes().find(id) == grid_.addedNodes().end())
if(stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
{
std::map<int, Signature>::const_iterator jter = stats.getSignatures().find(id);
if(jter != stats.getSignatures().end() && jter->second.sensorData().gridCellSize() > 0.0f)
{
cv::Mat groundCells, obstacleCells, emptyCells;
jter->second.sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
grid_.addToCache(id, groundCells, obstacleCells, emptyCells);
}
}
else
{
// Assume that older nodes are already added to map
break;
cv::Mat groundCells, obstacleCells, emptyCells;
stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
grid_.addToCache(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells);
}
}
if(grid_.addedNodes().size() || grid_.cacheSize())
{
grid_.update(stats.poses());
examples/WifiMapping/MapBuilder.h
+2 -2
View File
@@ -191,9 +191,9 @@ protected Q_SLOTS:
}
cloudViewer_->setCloudVisibility(cloudName, true);
}
else if(uContains(stats.getSignatures(), iter->first))
else if(iter->first == stats.getLastSignatureData().id())
{
Signature s = stats.getSignatures().at(iter->first);
Signature s = stats.getLastSignatureData();
s.sensorData().uncompressData(); // make sure data is uncompressed
// Add the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
examples/WifiMapping/MapBuilderWifi.h
+16 -22
View File
@@ -76,26 +76,19 @@ protected Q_SLOTS:
//============================
// Add WIFI symbols
//============================
std::map<double, int> nodeStamps; // <stamp, id>
// Sort stamps by stamps->id
nodeStamps_.insert(std::make_pair(stats.getLastSignatureData().getStamp(), stats.getLastSignatureData().id()));
for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
iter!=stats.getSignatures().end();
++iter)
if(!stats.getLastSignatureData().sensorData().userDataRaw().empty())
{
// Sort stamps by stamps->id
nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
UASSERT(stats.getLastSignatureData().sensorData().userDataRaw().type() == CV_64FC1 &&
stats.getLastSignatureData().sensorData().userDataRaw().cols == 2 &&
stats.getLastSignatureData().sensorData().userDataRaw().rows == 1);
if(!iter->second.sensorData().userDataRaw().empty())
{
UASSERT(iter->second.sensorData().userDataRaw().type() == CV_64FC1 &&
iter->second.sensorData().userDataRaw().cols == 2 &&
iter->second.sensorData().userDataRaw().rows == 1);
// format [int level, double stamp]
int level = iter->second.sensorData().userDataRaw().at<double>(0);
double stamp = iter->second.sensorData().userDataRaw().at<double>(1);
wifiLevels_.insert(std::make_pair(stamp, level));
}
// format [int level, double stamp]
int level = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(0);
double stamp = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(1);
wifiLevels_.insert(std::make_pair(stamp, level));
}
int id = 0;
@@ -103,15 +96,15 @@ protected Q_SLOTS:
{
// The Wifi value may be taken between two nodes, interpolate its position.
double stampWifi = iter->first;
std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
std::map<double, int>::iterator previousNode = nodeStamps_.lower_bound(stampWifi); // lower bound of the stamp
if(previousNode!=nodeStamps_.end() && previousNode->first > stampWifi && previousNode != nodeStamps_.begin())
{
--previousNode;
}
std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
std::map<double, int>::iterator nextNode = nodeStamps_.upper_bound(stampWifi); // upper bound of the stamp
if(previousNode != nodeStamps.end() &&
nextNode != nodeStamps.end() &&
if(previousNode != nodeStamps_.end() &&
nextNode != nodeStamps_.end() &&
previousNode->second != nextNode->second &&
uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
{
@@ -185,6 +178,7 @@ protected Q_SLOTS:
private:
std::map<double, int> wifiLevels_;
std::map<double, int> nodeStamps_; // <stamp, id>
};
examples/WifiMapping/main.cpp
+40 -3
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/RtabmapThread.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/OdometryThread.h"
#include "rtabmap/utilite/UEventsManager.h"
@@ -46,7 +47,7 @@ void showUsage()
"Options:\n"
" -i \"name\" Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
" -m Enable mirroring of the camera image.\n"
" -d # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
" -d # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2\n\n");
exit(1);
}
@@ -87,9 +88,9 @@ int main(int argc, char * argv[])
if(i < argc)
{
driver = atoi(argv[i]);
if(driver < 0 || driver > 4)
if(driver < 0 || driver > 8)
{
UERROR("driver should be between 0 and 4.");
UERROR("driver should be between 0 and 8.");
showUsage();
}
}
@@ -147,6 +148,42 @@ int main(int argc, char * argv[])
}
camera = new CameraOpenNICV(true, 0, opticalRotation);
}
else if (driver == 5)
{
if (!CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD, 0, opticalRotation);
}
else if (driver == 6)
{
if (!CameraStereoZed::available())
{
UERROR("Not built with ZED SDK support...");
exit(-1);
}
camera = new CameraStereoZed(0, 2, 1, 1, 100, false, 0, opticalRotation);
}
else if (driver == 7)
{
if (!CameraRealSense::available())
{
UERROR("Not built with RealSense support...");
exit(-1);
}
camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
}
else if (driver == 8)
{
if (!CameraRealSense2::available())
{
UERROR("Not built with RealSense2 support...");
exit(-1);
}
camera = new CameraRealSense2("", 0, opticalRotation);
}
else
{
camera = new rtabmap::CameraOpenni("", 0, opticalRotation);