Integrating Modular SLAM Pipeline into OpenCV Contrib #4043
+3,046
−1
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
abhishek-gola
requested review from
abhishek-gola,
asmorkalov,
gursimarsingh and
vpisarev
@QueenofUSSR build is failing. |
|
@QueenofUSSR, can you add some tests and a sample? |
Author
Okay, we will upload some tests and a visualized sample soon! |
Comment on lines
+16
to
+17
| // Adaptive Non-Maximal Suppression (ANMS) | ||
| static void anms(const std::vector<KeyPoint> &in, std::vector<KeyPoint> &out, int maxFeatures) |
There was a problem hiding this comment.
Please rename it to something like adaptiveNonMaxSuppression
Comment on lines
+39
to
+72
| bool ensureDirectoryExists(const std::string &dir){ | ||
| if(dir.empty()) return false; | ||
| #if HAVE_STD_FILESYSTEM | ||
| try{ | ||
| fs::path p(dir); | ||
| return fs::create_directories(p) || fs::exists(p); | ||
| }catch(...){ return false; } | ||
| #else | ||
| std::string tmp = dir; | ||
| if(tmp.empty()) return false; | ||
| while(tmp.size() > 1 && tmp.back() == '/') tmp.pop_back(); | ||
| std::string cur; | ||
| if(!tmp.empty() && tmp[0] == '/') cur = "/"; | ||
| size_t pos = 0; | ||
| while(pos < tmp.size()){ | ||
| size_t next = tmp.find('/', pos); | ||
| std::string part = (next == std::string::npos) ? tmp.substr(pos) : tmp.substr(pos, next-pos); | ||
| if(!part.empty()){ | ||
| if(cur.size() > 1 && cur.back() != '/') cur += '/'; | ||
| cur += part; | ||
| if(mkdir(cur.c_str(), 0755) != 0){ | ||
| if(errno == EEXIST){ /* ok */ } | ||
| else { | ||
| struct stat st; | ||
| if(stat(cur.c_str(), &st) == 0 && S_ISDIR(st.st_mode)){ | ||
| // ok | ||
| } else return false; | ||
| } | ||
| } | ||
| } | ||
| if(next == std::string::npos) break; | ||
| pos = next + 1; | ||
| } | ||
| return true; |
There was a problem hiding this comment.
Use opencv's filesystem <opencv2/core/utils/filesystem.hpp> it has inbuilt functions like createDirectories and isDirectory
Comment on lines
+26
to
+36
| for(int i=0;i<N;++i){ | ||
| for(int j=0;j<N;++j){ | ||
| if(in[j].response > in[i].response){ | ||
| float dx = in[i].pt.x - in[j].pt.x; | ||
| float dy = in[i].pt.y - in[j].pt.y; | ||
| float d2 = dx*dx + dy*dy; | ||
| if(d2 < radius[i]) radius[i] = d2; | ||
| } | ||
| } | ||
| // if no stronger keypoint exists, radius[i] stays INF | ||
| } |
There was a problem hiding this comment.
we can use parallel_for_ loop here.
Comment on lines
+88
to
+91
| try{ | ||
| calcOpticalFlowPyrLK(prevGray, image, prevPts, trackedPts, status, err, Size(21,21), 3, | ||
| TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), 0, 1e-4); | ||
| } catch(...) { status.clear(); } |
There was a problem hiding this comment.
Please format the catch part better, log the proper error using CV_Error.
Comment on lines
+154
to
+159
| Mat P2(3, 4, CV_64F); | ||
| for(int i = 0; i < 3; ++i) { | ||
| for(int j = 0; j < 3; ++j) P2.at<double>(i,j) = R.at<double>(i,j); | ||
| P2.at<double>(i, 3) = t.at<double>(i, 0); | ||
| } | ||
|
|
There was a problem hiding this comment.
cv::Mat P2;
cv::hconcat(R, t, P2);
It can also be done this way.
| catch(...) { points4D.release(); } | ||
| if(points4D.empty()) return newPoints; | ||
| Mat p4d64; | ||
| if(points4D.type() != CV_64F) points4D.convertTo(p4d64, CV_64F); else p4d64 = points4D; |
Comment on lines
+278
to
+307
| // Compute median descriptor (for binary descriptors, use majority voting per bit) | ||
| if(descriptors[0].type() == CV_8U) { | ||
| // Binary descriptor (ORB) | ||
| int bytes = descriptors[0].cols; | ||
| Mat median = Mat::zeros(1, bytes, CV_8U); | ||
|
|
||
| for(int b = 0; b < bytes; ++b) { | ||
| int bitCount[8] = {0}; | ||
| for(const auto &desc : descriptors) { | ||
| uchar byte = desc.at<uchar>(0, b); | ||
| for(int bit = 0; bit < 8; ++bit) { | ||
| if(byte & (1 << bit)) bitCount[bit]++; | ||
| } | ||
| } | ||
|
|
||
| uchar medianByte = 0; | ||
| int threshold = static_cast<int>(descriptors.size()) / 2; | ||
| for(int bit = 0; bit < 8; ++bit) { | ||
| if(bitCount[bit] > threshold) { | ||
| medianByte |= (1 << bit); | ||
| } | ||
| } | ||
| median.at<uchar>(0, b) = medianByte; | ||
| } | ||
|
|
||
| mp.descriptor = median; | ||
| } else { | ||
| // Fallback: use first descriptor | ||
| mp.descriptor = descriptors[0].clone(); | ||
| } |
There was a problem hiding this comment.
we can parallelize this loop too
Comment on lines
+246
to
+267
| for (int kfId : localKfIndices) { | ||
| if (fixedSet.find(kfId) != fixedSet.end()) continue; | ||
| if (kfId < 0 || kfId >= static_cast<int>(keyframes.size())) continue; | ||
| KeyFrame &kf = keyframes[kfId]; | ||
| // Build matchedMpIndices: for each keypoint in this KF, which mappoint index it corresponds to | ||
| std::vector<int> matchedMpIndices(kf.kps.size(), -1); | ||
| for (size_t mpIdx = 0; mpIdx < mappoints.size(); ++mpIdx) { | ||
| const MapPoint &mp = mappoints[mpIdx]; | ||
| if (mp.isBad) continue; | ||
| for (const auto &obs : mp.observations) { | ||
| if (obs.first == kfId) { | ||
| int kpIdx = obs.second; | ||
| if (kpIdx >= 0 && kpIdx < static_cast<int>(matchedMpIndices.size())) | ||
| matchedMpIndices[kpIdx] = static_cast<int>(mpIdx); | ||
| } | ||
| } | ||
| } | ||
| std::vector<bool> inliers; | ||
| // Use the same number of iterations as outer loop for RANSAC attempts | ||
| optimizePose(kf, mappoints, matchedMpIndices, fx, fy, cx, cy, inliers, std::max(20, iterations)); | ||
| } | ||
| } |
There was a problem hiding this comment.
Avoid this nested loop of 3 for loop calls, you can precompute matchedMpIndices.
Comment on lines
+174
to
+198
| auto ratioKeep = [&](const std::vector<std::vector<DMatch>>& knn, bool forward) { | ||
| std::vector<DMatch> filtered; | ||
| for(size_t qi=0; qi<knn.size(); ++qi){ | ||
| if(knn[qi].empty()) continue; | ||
| DMatch best = knn[qi][0]; | ||
| float ratio = 0.75f; | ||
| if(knn[qi].size() >= 2){ | ||
| if(knn[qi][1].distance > 0) { | ||
| if(best.distance / knn[qi][1].distance > ratio) continue; | ||
| } | ||
| } | ||
| // mutual check | ||
| int t = forward ? best.trainIdx : (int)qi; | ||
| // find reverse match for t | ||
| const auto &rev = forward ? knn21 : knn12; | ||
| if(t < 0 || t >= (int)rev.size() || rev[t].empty()) continue; | ||
| DMatch rbest = rev[t][0]; | ||
| if((forward && rbest.trainIdx == (int)qi) || (!forward && rbest.trainIdx == best.queryIdx)){ | ||
| filtered.push_back(best); | ||
| } | ||
| } | ||
| return filtered; | ||
| }; | ||
| std::vector<DMatch> goodMatches = ratioKeep(knn12, true); | ||
|
|
There was a problem hiding this comment.
why do we need this lambda function? you can use the code directly.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Pull Request Readiness Checklist
See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request
Patch to opencv_extra has the same branch name.
Summary
This PR adds the SLAM module (slam) providing a compact visual odometry and small-scale SLAM toolkit. The module includes feature extraction, matching, two-view initialization, pose estimation, local mapping (MapManager / MapPoint), a simple optimizer (local BA with SFM fallback and optional g2o support), visualizer/top-down trajectory, localizer (PnP-based relocalization), and utilities for loading image sequences and intrinsics.
Motivation
Offer a lightweight, self-contained SLAM implementation that demonstrates how to use OpenCV building blocks for VO/SLAM research and examples. Useful as an educational reference and quick prototyping base for researchers who want to extend or compare with other methods (ORB-SLAM family, etc.). It also provides a straightforward path to enable g2o when available for robust BA.
What changed / what’s included
slam.hpp— module aggregatorvo.hpp— VisualOdometry wrapperdata_loader.hpp— sequence loader with YAML intrinsics parsingfeature.hpp— FeatureExtractor (ORB + ANMS + flow-aware selection)matcher.hpp— Matcher (ratio test, bucketing, mutual check)initializer.hpp— Two-view initializer using H/F decomposition + triagulationpose.hpp— PoseEstimator (essential matrix + recoverPose)map.hpp&keyframe.hpp— MapManager, MapPoint, KeyFramelocalizer.hpp— PnP-based relocalizeroptimizer.hpp— Optimizer (local BA using either g2o or OpenCV-based SFM fallback)visualizer.hpp— simple frame/top-down visualization and trajectory savingImplementation (under src/):
vo.cpp— main VisualOdometry run loop, backend thread for local BA, CSV diagnostics, initialization + tracking + triangulation pipelinedata_loader.cpp— filesystem/glob image enumerator, optional sensor.yaml intrinsics parsing (small comment fix in this PR)feature.cpp,matcher.cpp,initializer.cpp,pose.cpp,optimizer.cpp,visualizer.cpp,localizer.cpp— corresponding implementations described above