From 8682026396bb6043b1a54194d5b69ca1192b46c1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 Oct 2021 11:17:42 -0400 Subject: [PATCH] Rtabmap: when optimizing graph, removed guess for rootid to avoid global rotation drift over time. CameraStereoImages: set BayerMode also to right image. GraphView: fixed 0 width for line inside NodeItem. Reprocess: added -loc_null and -gt options. --- corelib/src/Odometry.cpp | 3 -- corelib/src/Rtabmap.cpp | 4 +- corelib/src/camera/CameraImages.cpp | 2 +- corelib/src/camera/CameraStereoImages.cpp | 1 + guilib/src/GraphViewer.cpp | 51 ++++++++++++++++------- tools/Reprocess/main.cpp | 40 +++++++++++++++++- 6 files changed, 78 insertions(+), 23 deletions(-) diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 051452fe..f78e1fe9 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -303,9 +303,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet orientation* data.imu().localTransform().rotation().inverse(); - IMU imu2 = data.imu(); - imu2.convertToBaseFrame(); - if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f && this->framesProcessed() == 0) { diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index cbe272a9..d47e422b 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -4418,9 +4418,9 @@ std::map Rtabmap::optimizeGraph( { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - // Apply guess poses (if some) + // Apply guess poses (if some), ignore for rootid to avoid origin drifting std::map::const_iterator foundGuess = guessPoses.find(iter->first); - if(foundGuess!=guessPoses.end()) + if(foundGuess!=guessPoses.end() && iter->first != fromId) { iter->second = foundGuess->second; } diff --git a/corelib/src/camera/CameraImages.cpp b/corelib/src/camera/CameraImages.cpp index 894f1e61..c5817d08 100644 --- a/corelib/src/camera/CameraImages.cpp +++ b/corelib/src/camera/CameraImages.cpp @@ -862,7 +862,7 @@ SensorData CameraImages::captureImage(CameraInfo * info) cv::cvtColor(img, out, CV_BGRA2BGR); img = out; } - else if(_bayerMode >= 0 && _bayerMode <=3) + else if(!img.empty() && _bayerMode >= 0 && _bayerMode <=3) { cv::Mat debayeredImg; try diff --git a/corelib/src/camera/CameraStereoImages.cpp b/corelib/src/camera/CameraStereoImages.cpp index 8e94d63d..2ceed75d 100644 --- a/corelib/src/camera/CameraStereoImages.cpp +++ b/corelib/src/camera/CameraStereoImages.cpp @@ -166,6 +166,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info) { if(camera2_) { + camera2_->setBayerMode(this->getBayerMode()); right = camera2_->takeImage(info); } else diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index fa629ed4..e0b0c93f 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -67,7 +67,7 @@ class NodeItem: public QGraphicsEllipseItem { public: // in meter - NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane) : + NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane, float linkWidth) : QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)), _id(id), _mapId(mapId), @@ -82,6 +82,9 @@ public: pose.getEulerAngles(r, p, yaw); radius*=100.0f; _line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this); + QPen pen = _line->pen(); + pen.setWidth(linkWidth*100.0f); + _line->setPen(pen); } virtual ~NodeItem() {} @@ -94,7 +97,9 @@ public: b.setColor(color); this->setBrush(b); - _line->setPen(QPen(QColor(255-color.red(), 255-color.green(), 255-color.blue()))); + QPen pen = _line->pen(); + pen.setColor(QColor(255-color.red(), 255-color.green(), 255-color.blue())); + _line->setPen(pen); } void setRadius(float radius) @@ -157,8 +162,8 @@ private: class NodeGPSItem: public NodeItem { public: - NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane) : - NodeItem(id, mapId, pose, radius, -1, plane), + NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane, float linkWidth) : + NodeItem(id, mapId, pose, radius, -1, plane, linkWidth), _gps(gps) { } @@ -490,7 +495,7 @@ void GraphViewer::updateGraph(const std::map & poses, { // create node item const Transform & pose = iter->second; - NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane); + NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth); this->scene()->addItem(item); item->setZValue(iter->first<0?21:20); item->setColor(iter->first<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor); @@ -710,7 +715,7 @@ void GraphViewer::updateGTGraph(const std::map & poses) { // create node item const Transform & pose = iter->second; - NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane); + NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth); this->scene()->addItem(item); item->setZValue(20); item->setColor(_gtPathColor); @@ -745,15 +750,29 @@ void GraphViewer::updateGTGraph(const std::map & poses) } if(linkItem == 0) { - //create a link item - linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane); - QPen p = linkItem->pen(); - p.setWidthF(_linkWidth*100.0f); - linkItem->setPen(p); - linkItem->setZValue(10); - this->scene()->addItem(linkItem); - linkItem->setParentItem(_gtGraphRoot); - _gtLinkItems.insert(iterPrevious->first, linkItem); + bool linkFound = iter->first - iterPrevious->first == 1; // if consecutive, add link + for(QMultiMap::iterator kter = _linkItems.find(iterPrevious->first); + kter!=_linkItems.end() && kter.key()==iterPrevious->first && !linkFound; + ++kter) + { + if(kter.value()->from() == iterPrevious->first && kter.value()->to() == iter->first) + { + linkFound = true; + } + } + + if(linkFound) + { + //create a link item + linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane); + QPen p = linkItem->pen(); + p.setWidthF(_linkWidth*100.0f); + linkItem->setPen(p); + linkItem->setZValue(10); + this->scene()->addItem(linkItem); + linkItem->setParentItem(_gtGraphRoot); + _gtLinkItems.insert(iterPrevious->first, linkItem); + } } if(linkItem) { @@ -840,7 +859,7 @@ void GraphViewer::updateGPSGraph( // create node item const Transform & pose = iter->second; UASSERT(gpsValues.find(iter->first) != gpsValues.end()); - NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane); + NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth); this->scene()->addItem(item); item->setZValue(20); item->setColor(_gpsPathColor); diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index 60f450e3..9d5c6d1a 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -55,7 +55,8 @@ void showUsage() " rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n" "\n" " For the second example, only parameters from the first database are used.\n" - " If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database.\n" + " If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n" + " then localization-only is done with next databases against the first one.\n" " To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n" " To upgrade version of an old database to newest version:\n" " rtabmap-reprocess --Db/TargetVersion \"\" \"input.db\" \"output.db\"\n" @@ -68,6 +69,9 @@ void showUsage() " arguments, they overwrite those in config file and the database.\n" " -start # Start from this node ID.\n" " -stop # Last node to process.\n" + " -loc_null On localization mode, reset localization pose to null and map correction to identity between sessions.\n" + " -gt When reprocessing a single database, load its original optimized graph, then \n" + " set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n" " -g2 Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\". Use with -db to save in database.\n" " -g3 Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n" " -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n" @@ -221,6 +225,8 @@ int main(int argc, char * argv[]) int startId = 0; int stopId = 0; int framesToSkip = 0; + bool locNull = false; + bool originalGraphAsGT = false; bool scanFromDepth = false; int scanDecimation = 1; float scanRangeMin = 0.0f; @@ -297,6 +303,16 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0) + { + locNull = true; + printf("In localization mode, when restarting a new session, the current localization pose is set to null (-loc_null option).\n"); + } + else if(strcmp(argv[i], "-gt") == 0 || strcmp(argv[i], "--gt") == 0) + { + originalGraphAsGT = true; + printf("Original graph is used as ground truth for output database (-gt option).\n"); + } else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0) { exportPoses = true; @@ -529,6 +545,13 @@ int main(int argc, char * argv[]) { totalIds = ids.size(); } + + std::map gt; + if(databases.size() == 1 && originalGraphAsGT) + { + gt = dbDriver->loadOptimizedPoses(); + } + dbDriver->closeConnection(false); // Count remaining ids in the other databases @@ -573,6 +596,11 @@ int main(int argc, char * argv[]) Rtabmap rtabmap; rtabmap.init(parameters, outputDatabasePath); + if(!incrementalMemory && locNull) + { + rtabmap.setInitialPose(Transform()); + } + bool rgbdEnabled = Parameters::defaultRGBDEnabled(); Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled); bool odometryIgnored = !rgbdEnabled; @@ -623,8 +651,18 @@ int main(int argc, char * argv[]) lastLocalizationOdomPose = info.odomPose; } rtabmap.triggerNewMap(); + if(!incrementalMemory && locNull) + { + rtabmap.setInitialPose(Transform()); + } inMotion = true; } + + if(originalGraphAsGT) + { + data.setGroundTruth(gt.find(data.id()) != gt.end()?gt.at(data.id()):Transform()); + } + UTimer t; if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats)) {