diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index a58b719f20..a3e785b6a4 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,11 +4,10 @@ set(debug_modules "") if(DEBUG_opencv_ptcloud) list(APPEND debug_modules opencv_highgui) endif() -ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} +ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_features opencv_video ${debug_modules} WRAP java objc python js ) ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES}) -ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) if(NOT HAVE_EIGEN) message(STATUS "Geometry: Eigen support is disabled. Eigen is Required for Posegraph optimization") diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index d8ae7fc470..d5e3a11034 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -35,12 +35,12 @@ Ptr VisualOdometry::create( // Constructor VisualOdometryImpl::VisualOdometryImpl( - const Ptr& detector, - const Ptr& matcher, + const Ptr& detector_, + const Ptr& matcher_, const Mat& cameraMatrix, const Mat& distCoeffs, - const OdometryParams& params) - : detector(detector), matcher(matcher), params(params) + const OdometryParams& params_) + : detector(detector_), matcher(matcher_), params(params_) { cameraMatrix.convertTo(K, CV_64F); if (!distCoeffs.empty()) diff --git a/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp index 8f6b0c29ee..cc5c806b37 100644 --- a/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp +++ b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp @@ -132,10 +132,10 @@ bool VisualOdometryImpl::shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw Point3d cCur = detail::cameraCenterWorld(T_cw); Point3d cLast = detail::cameraCenterWorld(lastKf->poseCw); Point3d d = cCur - cLast; - double dist = std::sqrt(d.dot(d)); - if (dist > params.kfTransThresh) + double transDist = std::sqrt(d.dot(d)); + if (transDist > params.kfTransThresh) { - reason = format("trans=%.3f", dist); + reason = format("trans=%.3f", transDist); return true; } diff --git a/modules/ptcloud/src/slam/odometry/vo_tracking.cpp b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp index f27147033a..6ceb1e9933 100644 --- a/modules/ptcloud/src/slam/odometry/vo_tracking.cpp +++ b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp @@ -218,9 +218,13 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) if (!hasPrevFrame || prevFrame.image.empty()) return false; const Frame& prev = prevFrame; - if (prev.undistKpts.empty()) return false; + if (prev.keypoints.empty()) return false; + + // LK runs on the raw imgs + std::vector prevPts; + prevPts.reserve(prev.keypoints.size()); + for (const auto& kp : prev.keypoints) prevPts.push_back(kp.pt); - std::vector prevPts(prev.undistKpts.begin(), prev.undistKpts.end()); std::vector curPts; std::vector status; std::vector err; @@ -229,6 +233,13 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) status, err, Size(21, 21), 3, TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01)); + // PnP uses K with no distortion + std::vector curUndist; + if (!dist.empty()) + undistortPoints(curPts, curUndist, K, dist, noArray(), K); + else + curUndist = curPts; + std::vector obj; std::vector img; obj.reserve(prevPts.size()); img.reserve(prevPts.size()); @@ -239,7 +250,7 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) MapPoint* mp = prev.mapPoints[i]; if (!mp || mp->bad) continue; obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); - img.push_back(curPts[i]); + img.push_back(curUndist[i]); } if ((int)obj.size() < params.opticalFlowMinInliers) diff --git a/modules/ptcloud/src/slam/precomp.hpp b/modules/ptcloud/src/slam/precomp.hpp index d489fa9c0d..dc9fc8dff5 100644 --- a/modules/ptcloud/src/slam/precomp.hpp +++ b/modules/ptcloud/src/slam/precomp.hpp @@ -10,12 +10,8 @@ #include "opencv2/ptcloud/slam.hpp" #include "opencv2/core.hpp" -#include "opencv2/core/utility.hpp" -#include "opencv2/core/utils/filesystem.hpp" -#include "opencv2/core/utils/logger.hpp" #include "opencv2/core/private.hpp" #include "opencv2/imgproc.hpp" -#include "opencv2/imgcodecs.hpp" #include "opencv2/features.hpp" #include "opencv2/geometry.hpp" #include "opencv2/video/tracking.hpp" diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt index f87637b768..2ce39a8df0 100644 --- a/samples/slam/CMakeLists.txt +++ b/samples/slam/CMakeLists.txt @@ -4,6 +4,7 @@ set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS opencv_core opencv_dnn opencv_features + opencv_imgcodecs opencv_ptcloud) ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 423aa6a8d2..7f170139ee 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -5,7 +5,6 @@ #include #include -#include #include #include #include @@ -13,6 +12,7 @@ #include #include #include +#include #include "../dnn/common.hpp" @@ -21,15 +21,6 @@ using namespace std; namespace { -// path helper: joins a directory and filename, tolerating a dir with/without a trailing slash -String joinPath(const String& dir, const String& name) -{ - if (dir.empty()) return name; - char last = dir.back(); - if (last == '/' || last == '\\') return dir + name; - return dir + "/" + name; -} - // path helper: strips the directory part off a path String basenameOf(const String& path) { @@ -93,7 +84,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, cv::utils::fs::createDirectories(outputFolder); { - std::ofstream f(joinPath(outputFolder, "camera.txt").c_str()); + std::ofstream f(cv::utils::fs::join(outputFolder, "camera.txt").c_str()); int width = 0, height = 0; if (!vo.getMap().keyframes().empty()) { @@ -111,7 +102,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, } { - std::ofstream f(joinPath(outputFolder, "point3d.txt").c_str()); + std::ofstream f(cv::utils::fs::join(outputFolder, "point3d.txt").c_str()); f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; f.setf(std::ios::scientific); f.precision(9); for (slam::MapPoint* mp : vo.getMap().mapPoints()) @@ -124,7 +115,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, } { - std::ofstream f(joinPath(outputFolder, "images.txt").c_str()); + std::ofstream f(cv::utils::fs::join(outputFolder, "images.txt").c_str()); const auto& traj = vo.getTrajectory(); f << "# Image list with two lines of data per image:\n" << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" @@ -170,7 +161,8 @@ const string param_keys = "{ fx | 718.856 | Camera focal length X }" "{ fy | 718.856 | Camera focal length Y }" "{ cx | 607.1928 | Camera principal point X }" - "{ cy | 185.2157 | Camera principal point Y }"; + "{ cy | 185.2157 | Camera principal point Y }" + "{ dist | | Lens distortion coeffs, comma-separated k1,k2,p1,p2[,k3,...] (default: none) }"; const string backend_keys = format( "{ backend | default | Choose one of computation backends: " @@ -223,6 +215,23 @@ int main(int argc, char** argv) 0., parser.get("fy"), parser.get("cy"), 0., 0., 1.); + // Optional lens distortion (k1,k2,p1,p2[,k3,...]); empty means no distortion. + Mat distCoeffs; + { + std::stringstream ss(parser.get("dist")); + std::vector coeffs; + String tok; + while (std::getline(ss, tok, ',')) + { + const size_t b = tok.find_first_not_of(" \t"); + const size_t e = tok.find_last_not_of(" \t"); + if (b == String::npos) continue; + coeffs.push_back(std::stod(tok.substr(b, e - b + 1))); + } + if (!coeffs.empty()) + distCoeffs = Mat(coeffs, true).reshape(1, 1); + } + ALIKED::Params detParams; detParams.inputSize = Size(640, 640); detParams.engine = dnn::ENGINE_NEW; @@ -234,7 +243,7 @@ int main(int argc, char** argv) backendId, targetId); slam::OdometryParams voParams; - + voParams.minInitParallaxDeg = 1.5; voParams.minInitPoints = 50; voParams.pnpReprojThresh = 4.0; @@ -242,7 +251,7 @@ int main(int argc, char** argv) voParams.localMapTopK = 10; auto vo = slam::VisualOdometry::create( - detector, matcher, Mat(K), Mat(), voParams); + detector, matcher, Mat(K), distCoeffs, voParams); const std::vector imgFiles = listImageFiles(imagesDir);