slam VO review2 minor changes/fixes

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")

modules/ptcloud/src/slam/odometry/visual_odometry.cpp

@@ -35,12 +35,12 @@ Ptr<VisualOdometry> VisualOdometry::create(
 // Constructor
 
 VisualOdometryImpl::VisualOdometryImpl(
-    const Ptr<Feature2D>& detector,
-    const Ptr<DescriptorMatcher>& matcher,
+    const Ptr<Feature2D>& detector_,
+    const Ptr<DescriptorMatcher>& 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())

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;
     }
 

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<Point2f> prevPts;
+    prevPts.reserve(prev.keypoints.size());
+    for (const auto& kp : prev.keypoints) prevPts.push_back(kp.pt);
 
-    std::vector<Point2f> prevPts(prev.undistKpts.begin(), prev.undistKpts.end());
     std::vector<Point2f> curPts;
     std::vector<uchar> status;
     std::vector<float> 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<Point2f> curUndist;
+    if (!dist.empty())
+        undistortPoints(curPts, curUndist, K, dist, noArray(), K);
+    else
+        curUndist = curPts;
+
     std::vector<Point3f> obj; std::vector<Point2f> 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)

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"