Miscellaneous SLAM tweaks

 * Fix pos. uncertainty computation
 * Improve determinism across machines
 * Print errors for bad SLAM setup
 * Simplify odometry uncertainty handling with odometryPriorFixed=false
 * Add keyframeDecisionAlways option (off by default)
 * Add options to disable key frame and map point culling
 * Add command line option for turning off extra ORBs
 * Minor refactoring and cleanup

Co-authored-by: Pekka Rantalankila <pekka.rantalankila@gmail.com>

Author: oseiskar

bow_index.cpp

@@ -13,6 +13,10 @@ BowVocabulary createBowVocabulary(std::string vocabularyPath) {
     BowVocabulary bowVocabulary;
     assert(!vocabularyPath.empty());
     const std::string inputSuffix = vocabularyPath.substr(util::removeFileSuffix(vocabularyPath).size());
+    {
+        std::ifstream file(vocabularyPath);
+        assert(file.is_open() && "Failed to find BoW vocabulary. Run `scripts/download_orb_vocab.sh`.");
+    }
     if (inputSuffix == ".txt") {
         log_debug("Loading BoW vocabulary from TEXT file %s", vocabularyPath.c_str());
         bowVocabulary.loadFromTextFile(vocabularyPath);

keyframe.cpp

@@ -81,9 +81,9 @@ Keyframe::Keyframe(const MapperInput &mapperInput) :
     id(KfId(mapperInput.poseTrail.at(0).frameNumber)),
     previousKfId(KfId(-1)),
     nextKfId(KfId(-1)),
-    poseCW(Eigen::Matrix4d::Identity()),
-    origPoseCW(mapperInput.poseTrail[0].pose),
-    uncertainty(mapperInput.poseTrail[0].uncertainty),
+    poseCW(Eigen::Matrix4d::Zero()), // not valid yet
+    smoothPoseCW(Eigen::Matrix4d::Zero()), // not valid yet
+    uncertainty(mapperInput.poseTrail.at(0).uncertainty),
     t(mapperInput.poseTrail[0].t),
     hasFullFeatures(false)
 {
@@ -141,7 +141,7 @@ Keyframe::Keyframe(const Keyframe &kf) :
     mapPoints(kf.mapPoints),
     keyPointDepth(kf.keyPointDepth),
     poseCW(kf.poseCW),
-    origPoseCW(kf.origPoseCW),
+    smoothPoseCW(kf.smoothPoseCW),
     uncertainty(kf.uncertainty),
     t(kf.t)
 {}
@@ -234,8 +234,8 @@ Eigen::Vector3d Keyframe::cameraCenter() const {
     return worldToCameraMatrixCameraCenter(poseCW);
 }
 
-Eigen::Vector3d Keyframe::origPoseCameraCenter() const {
-    return worldToCameraMatrixCameraCenter(origPoseCW);
+Eigen::Vector3d Keyframe::smoothPoseCameraCenter() const {
+    return worldToCameraMatrixCameraCenter(smoothPoseCW);
 }
 
 void Keyframe::getFeaturesAround(const Eigen::Vector2f &point, float r, std::vector<size_t> &indices) {
@@ -312,7 +312,7 @@ std::string Keyframe::toString() {
     ss << "nextKfId=" << std::to_string(nextKfId.v);
     ss << ", points=" << std::to_string(shared->keyPoints.size());
     ss << ", poseCW=" << util::eigenToString(poseCW);
-    ss << ", origPoseCW=" << util::eigenToString(origPoseCW);
+    ss << ", smoothPoseCW=" << util::eigenToString(smoothPoseCW);
     ss << ", camera=" << shared->camera->serialize();
     ss << ", keyPoints=";
     keyPointVectorToStringstream(&ss, shared->keyPoints);

keyframe.hpp

@@ -136,7 +136,7 @@ class Keyframe {
     bool reproject(const Eigen::Vector3d &point, Eigen::Vector2f &reprojected) const;
 
     Eigen::Vector3d cameraCenter() const;
-    Eigen::Vector3d origPoseCameraCenter() const;
+    Eigen::Vector3d smoothPoseCameraCenter() const;
     inline Eigen::Matrix3d cameraToWorldRotation() const {
         return poseCW.topLeftCorner<3, 3>().transpose();
     }
@@ -170,11 +170,20 @@ class Keyframe {
     // KF positions in non-linear optimization.
     Eigen::Matrix4d poseCW;
 
-    // Pose computed by odometry with no (or minimal) influence from SLAM. It is used
-    // mainly to setup priors between subsequent keyframes in non-linear optimization.
-    Eigen::Matrix4d origPoseCW;
-
-    // Uncertainty matrix for position & rotation
+    // These poses are meant to form a smooth path so that differences of these
+    // from consecutive keyframes can be used as odometry priors
+    // Especially if the odometry result is very good, the unfiltered odometry
+    // output poses can be used for this purpose.
+    Eigen::Matrix4d smoothPoseCW;
+
+    // Accumulated uncertainty (for position and orientation separately).
+    // These are formed by summing covariance matrices that represent the uncertainty
+    // between consecutive key frames, which corresponds to the assumption / simplification
+    // that those noises are independent (which is not very accurate). However, this means
+    // that these uncertainty matrices form a total order: if t1 >= t2, then
+    // uncertainty2 - uncertainty1 is positive semidefinite. This simplifies the handling
+    // of dropped keyframes as you can always just subtract these and still get a valid
+    // covariance matrix
     Eigen::Matrix<double, 3, 6> uncertainty;
 
     double t;
@@ -197,7 +206,7 @@ class Keyframe {
             mapPoints,
             keyPointDepth,
             poseCW,
-            origPoseCW,
+            smoothPoseCW,
             uncertainty,
             t,
             hasFullFeatures

mapdb.cpp

@@ -38,48 +38,55 @@ std::shared_ptr<Keyframe> MapDB::insertNewKeyframeCandidate(
     const std::vector<slam::Pose> &poseTrail,
     const odometry::ParametersSlam &parameters)
 {
-    Eigen::Matrix4d pose;
+    Eigen::Matrix4d pose, smoothPose;
     std::shared_ptr<Keyframe> keyframe = std::move(keyframeUniq);
     Keyframe *previousKf = latestKeyframe();
     // debugPrintPoseTrail(poseTrail);
 
+    smoothPose = poseTrail[0].pose;
+    // TODO: to be more accurate, should uncertainty "deltas" over pose trail
+    Eigen::Matrix<double, 3, 6> curUncertainty = poseTrail[0].uncertainty * parameters.keyframeCandidateInterval;
+    // assert(curUncertainty.squaredNorm() > 1e-20); // matters only if used
+
     if (prevPoseKfId.v < 0) {
-        pose = keyframe->origPoseCW;
+        // First KF (or something bad happened): use odometry pose as-is
+        pose = poseTrail[0].pose;
         // Tip: Use this to change direction of SLAM map and debug transform issues.
         // pose.topLeftCorner<3, 3>() = pose.topLeftCorner<3, 3>() * Eigen::AngleAxisd(0.5 * M_PI, Vector3d::UnitZ());
         // pose.block<3, 1>(0, 3) << 4, 1, -2;
     }
     else {
         assert(previousKf);
-        // this "two-step" mechanism may be required to handle loop closures correctly
         Eigen::Matrix4d refPose = prevPose;
-        if (parameters.useVariableLengthDeltas) {
-            refPose = prevPoseToPrevKeyframeDelta * previousKf->poseCW;
-        }
 
-        Eigen::Matrix4d poseTilted, refPrevPose;
-        refPrevPose = prevInputPose;
+        // Could update prevPose to previousKf->poseCW here, but this does not play nice
+        // with loop closures or non-keyframes and the practical difference is very small
+
         if (parameters.useOdometryPoseTrailDelta) {
             // note that this can fail if in certain cases with long
             // stationarity and no new keyframes unless special care is taken
             const auto *prevPoseInPoseTrail = findInPoseTrail(poseTrail, prevPoseKfId);
             if (prevPoseInPoseTrail == nullptr) {
-                log_debug("keyframe %d not found in pose trail", prevPoseKfId.v);
+                log_warn("keyframe %d not found in pose trail", prevPoseKfId.v);
             } else {
                 assert(KfId(prevPoseInPoseTrail->frameNumber) != keyframe->id);
-                refPrevPose = prevPoseInPoseTrail->pose;
+                smoothPose = poseTrail[0].pose * prevPoseInPoseTrail->pose.inverse() * prevSmoothPose;
+                // No need to remove Z-axis tilt here. "smoothPose" should only be used for computing
+                // odometry priors between consecutive keyframes but never as an absolute pose
             }
         }
-        poseTilted = keyframe->origPoseCW * refPrevPose.inverse() * refPose;
 
+        Eigen::Matrix4d poseTilted = poseTrail[0].pose * prevInputPose.inverse() * refPose;
         if (parameters.removeOdometryTransformZAxisTilt) {
-            pose = removeZAxisTiltComparedToReference(poseTilted, keyframe->origPoseCW);
+            pose = removeZAxisTiltComparedToReference(poseTilted, smoothPose);
         } else {
             pose = poseTilted;
         }
     }
 
     keyframe->poseCW = pose;
+    keyframe->smoothPoseCW = smoothPose;
+    keyframe->uncertainty = prevUncertainty + curUncertainty;
 
     if (previousKf) {
         keyframe->previousKfId = previousKf->id;
@@ -106,7 +113,8 @@ MapDB::MapDB(const MapDB &mapDB) {
 
     prevPose = mapDB.prevPose;
     prevInputPose = mapDB.prevInputPose;
-    discardedUncertainty = mapDB.discardedUncertainty;
+    prevSmoothPose = mapDB.prevSmoothPose;
+    prevUncertainty = mapDB.prevUncertainty;
 
     nextMp = mapDB.nextMp;
     lastKfCandidateId = mapDB.lastKfCandidateId;
@@ -149,8 +157,8 @@ MapDB::MapDB(const MapDB &mapDB, const std::set<KfId> &activeKeyframes) {
 
     prevPose = mapDB.prevPose;
     prevInputPose = mapDB.prevInputPose;
-    prevPoseToPrevKeyframeDelta = mapDB.prevPoseToPrevKeyframeDelta;
-    discardedUncertainty = mapDB.discardedUncertainty;
+    prevSmoothPose = mapDB.prevSmoothPose;
+    prevUncertainty = mapDB.prevUncertainty;
 
     nextMp = mapDB.nextMp;
     prevPoseKfId = mapDB.prevPoseKfId;
@@ -225,45 +233,20 @@ Eigen::Matrix4d MapDB::poseDifference(KfId kfId1, KfId kfId2) const {
     assert(kfId1 <= kfId2);
     const Keyframe &kf1 = *keyframes.at(kfId1);
     const Keyframe &kf2 = *keyframes.at(kfId2);
-    return kf1.origPoseCW * kf2.origPoseCW.inverse();
+    return kf1.smoothPoseCW * kf2.smoothPoseCW.inverse();
 }
 
-void MapDB::updatePrevPose(
-    const Keyframe &currentKeyframe,
-    bool keyframeDecision,
-    const std::vector<slam::Pose> &poseTrail,
-    const odometry::Parameters &parameters)
+void MapDB::updatePrevPose(const Keyframe &currentKeyframe, const Eigen::Matrix4d &inputPose)
 {
-    if (!keyframeDecision && parameters.slam.useVariableLengthDeltas && findInPoseTrail(poseTrail, prevPoseKfId) == nullptr) {
-        log_debug("prevPoseKfId %d lost in pose trail: must update", prevPoseKfId.v);
-        // TODO: could rather try to update to an older pose isn the trail
-        // TODO: such odometry non-keyframes should also be removed from the map
-        keyframeDecision = true;
-    }
-
-    if (!keyframeDecision && parameters.slam.useVariableLengthDeltas) {
-        // TODO: this is overly complex
-        const int nextKfCandidateAge = currentKeyframe.id.v - prevPoseKfId.v + parameters.slam.keyframeCandidateInterval;
-        assert(nextKfCandidateAge > 0);
-        // TODO: +1? (not so serious here)
-        const int maxPoseTrailSize = parameters.odometry.cameraTrailLength - std::max(0, parameters.slam.delayIntervalMultiplier) * parameters.slam.keyframeCandidateInterval;
-        // log_debug("next %d/%d", nextKfCandidateAge, maxPoseTrailSize);
-        if (parameters.slam.useOdometryPoseTrailDelta && nextKfCandidateAge >= maxPoseTrailSize) {
-            log_debug("storing prevPose of non-KF %d: max pose trail length will be reached", currentKeyframe.id.v);
-        } else {
-            // log_debug(" skipping non-keyframe %d as prev pose", currentKeyframe.id.v);
-            return;
-        }
-    }
-
     // log_debug("storing keyframe %d as prev pose", currentKeyframe.id.v);
     prevPoseKfId = currentKeyframe.id;
-    prevInputPose = currentKeyframe.origPoseCW;
+    prevInputPose = inputPose;
+    prevSmoothPose = currentKeyframe.smoothPoseCW;
     prevPose = currentKeyframe.poseCW;
+    prevUncertainty = currentKeyframe.uncertainty;
 
     const auto *prevKf = latestKeyframe();
     assert(prevKf);
-    prevPoseToPrevKeyframeDelta = prevPose * prevKf->poseCW.inverse();
 }
 
 const MapDB& getMapWithId(MapId mapId, const MapDB &mapDB, const Atlas &atlas) {

mapdb.hpp

@@ -18,9 +18,10 @@ class MapDB {
 public:
 
     MapDB() :
-        discardedUncertainty(Eigen::MatrixXd::Zero(3, 6)),
         prevPose(Eigen::Matrix4d::Identity()),
-        prevInputPose(Eigen::Matrix4d::Identity()) {};
+        prevInputPose(Eigen::Matrix4d::Identity()),
+        prevUncertainty(Eigen::Matrix<double, 3, 6>::Zero())
+    {};
     MapDB(const MapDB &mapDB); // Copy constructor
     MapDB(const MapDB &mapDB, const std::set<KfId> &activeKeyframes); // Copy constructor that only copies given frames
 
@@ -29,8 +30,6 @@ class MapDB {
     std::map<TrackId, MpId> trackIdToMapPoint;
     std::vector<LoopClosureEdge> loopClosureEdges;
 
-    Eigen::MatrixXd discardedUncertainty;
-
     double firstKfTimestamp = -1.0;
 
     std::shared_ptr<Keyframe> insertNewKeyframeCandidate(
@@ -56,21 +55,16 @@ class MapDB {
 
     Eigen::Matrix4d poseDifference(KfId kfId1, KfId kfId2) const;
 
-    void updatePrevPose(
-        const Keyframe &currentKeyframe,
-        bool keyframeDecision,
-        const std::vector<slam::Pose> &poseTrail,
-        const odometry::Parameters &parameters);
+    void updatePrevPose(const Keyframe &currentKeyframe, const Eigen::Matrix4d &inputPose);
 
     // Visualization stuff stored here for convenience.
     std::map<MapKf, LoopStage> loopStages;
     std::vector<KfId> adjacentKfIds;
     std::map<MpId, MapPointRecord> mapPointRecords;
 
 private:
-    Eigen::Matrix4d prevPose;
-    Eigen::Matrix4d prevInputPose;
-    Eigen::Matrix4d prevPoseToPrevKeyframeDelta = Eigen::Matrix4d::Identity();
+    Eigen::Matrix4d prevPose, prevInputPose, prevSmoothPose;
+    Eigen::Matrix<double, 3, 6> prevUncertainty;
     int nextMp = 0;
     // id of the frame corresponding to prevPose & prevInput pose. May no longer exist
     KfId prevPoseKfId = KfId(-1);
@@ -89,7 +83,8 @@ class MapDB {
             loopClosureEdges,
             prevPose,
             prevInputPose,
-            discardedUncertainty,
+            prevSmoothPose,
+            prevUncertainty,
             firstKfTimestamp,
             nextMp,
             lastKfCandidateId,