slam-vo init

2026-07-29 15:23:05 +04:00 · 2026-06-05 18:00:13 +05:30
parent 527f01449d
commit 72b3fea7dd
13 changed files with 652 additions and 0 deletions
@@ -0,0 +1,14 @@
+# This file is part of OpenCV project.
+# It is subject to the license terms in the LICENSE file found in the top-level directory
+# of this distribution and at http://opencv.org/license.html.
+
+set(the_description "SLAM (Simultaneous Localization and Mapping) — visual odometry and mapping")
+
+ocv_define_module(slam
+    opencv_core
+    opencv_imgproc
+    opencv_imgcodecs
+    opencv_features
+    opencv_3d
+    OPTIONAL opencv_dnn
+    WRAP python)
@@ -0,0 +1,22 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef OPENCV_SLAM_HPP
+#define OPENCV_SLAM_HPP
+
+#include "slam/types.hpp"
+#include "slam/map.hpp"
+#include "slam/odometry_params.hpp"
+#include "slam/visual_odometry.hpp"
+
+/**
+@defgroup slam SLAM (Simultaneous Localization and Mapping)
+@{
+    @defgroup slam_odometry Visual Odometry
+    @brief Monocular visual odometry pipeline.
+@}
+*/
+
+#endif // OPENCV_SLAM_HPP
@@ -0,0 +1,73 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef OPENCV_SLAM_MAP_HPP
+#define OPENCV_SLAM_MAP_HPP
+
+#include "types.hpp"
+#include <opencv2/core.hpp>
+#include <vector>
+
+namespace cv { namespace slam {
+
+/** @brief Container for keyframes, map points, and the per-frame trajectory.
+
+    Map owns all KeyFrame and MapPoint objects. External code holds only
+    raw non-owning pointers. Ids are assigned automatically when the
+    incoming id field is negative.
+
+    @ingroup slam_odometry
+*/
+class CV_EXPORTS_W Map
+{
+public:
+    CV_WRAP Map();
+    ~Map();
+
+    /** @brief Add a keyframe; assigns id if kf.id < 0. Returns the assigned id. */
+    CV_WRAP int addKeyframe(KeyFrame& kf);
+
+    /** @brief Return keyframe by id, or nullptr. */
+    CV_WRAP KeyFrame* getKeyframe(int id);
+
+    /** @brief All keyframes in insertion order. */
+    CV_WRAP const std::vector<KeyFrame*>& keyframes() const;
+
+    CV_WRAP int numKeyframes() const;
+
+    /** @brief Add a map point; assigns id if mp.id < 0. Returns the assigned id. */
+    CV_WRAP int addMapPoint(MapPoint& mp);
+
+    /** @brief Return map point by id, or nullptr. */
+    CV_WRAP MapPoint* getMapPoint(int id);
+
+    /** @brief All live (non-bad) map points. */
+    CV_WRAP std::vector<MapPoint*> mapPoints() const;
+
+    CV_WRAP int numMapPoints() const;
+
+    /** @brief Wire a bidirectional KF-MP observation. */
+    CV_WRAP void addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp);
+
+    /** @brief Mark a map point bad and remove all cross-references. */
+    CV_WRAP void removeMapPoint(int mp_id);
+
+    /** @brief Append a world-to-camera pose to the trajectory. */
+    CV_WRAP void appendPose(const Matx44d& T_cw);
+
+    /** @brief All emitted world-to-camera poses in order. */
+    CV_WRAP const std::vector<Matx44d>& trajectory() const;
+
+    /** @brief Reset to empty state. */
+    CV_WRAP void clear();
+
+private:
+    struct Impl;
+    Ptr<Impl> impl_;
+};
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_MAP_HPP
@@ -0,0 +1,37 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef OPENCV_SLAM_ODOMETRY_PARAMS_HPP
+#define OPENCV_SLAM_ODOMETRY_PARAMS_HPP
+
+#include <opencv2/core.hpp>
+
+namespace cv { namespace slam {
+
+/** @brief Tunable parameters for the VisualOdometry pipeline.
+
+    All fields have sensible defaults. Override any subset before
+    passing to VisualOdometry::create().
+
+    @ingroup slam_odometry
+*/
+struct CV_EXPORTS_W_SIMPLE OdometryParams
+{
+    CV_WRAP OdometryParams() = default;
+
+    // --- Bootstrap (two-view init) -------------------------------------------
+
+    CV_PROP_RW int    min_init_inliers            = 80;    //!< Min essential-matrix RANSAC inliers.
+    CV_PROP_RW double min_init_parallax_deg       = 3.0;   //!< Min median parallax to trigger bootstrap.
+    CV_PROP_RW int    min_init_points             = 50;    //!< Min triangulated points to accept init.
+    CV_PROP_RW double hf_ratio_thresh             = 0.45;  //!< H/(H+F) ratio; > thresh uses Homography.
+    CV_PROP_RW double min_growth_parallax_deg     = 1.0;   //!< Min parallax for a new map point to survive.
+    CV_PROP_RW double essential_ransac_thresh     = 1.0;   //!< RANSAC reprojection threshold (px).
+    CV_PROP_RW double essential_ransac_confidence = 0.999; //!< RANSAC confidence for findEssentialMat.
+};
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP
@@ -0,0 +1,71 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef OPENCV_SLAM_TYPES_HPP
+#define OPENCV_SLAM_TYPES_HPP
+
+#include <opencv2/core.hpp>
+#include <map>
+#include <vector>
+
+namespace cv { namespace slam {
+
+/** @brief Current state of the VisualOdometry pipeline.
+    @ingroup slam_odometry
+*/
+enum OdometryState
+{
+    NOT_INITIALIZED = 0, //!< No frames processed yet.
+    INITIALIZING    = 1, //!< Reference frame set; waiting for bootstrap.
+    TRACKING        = 2  //!< Map initialised; localising via PnP.
+};
+
+/** @brief Feature data for one image frame.
+    @ingroup slam_odometry
+*/
+struct CV_EXPORTS_W_SIMPLE FrameFeatures
+{
+    std::vector<KeyPoint> keypoints;
+    Mat                   descriptors;
+    Size                  imageSize;
+};
+
+struct MapPoint;
+struct KeyFrame;
+
+/** @brief A triangulated 3-D landmark owned by Map.
+    @ingroup slam_odometry
+*/
+struct CV_EXPORTS_W_SIMPLE MapPoint
+{
+    int     id  = -1;
+    Point3d pos;
+    bool    bad = false;
+
+    std::map<KeyFrame*, int> observations; //!< Observing keyframe → keypoint index.
+};
+
+/** @brief A frame whose pose has been committed to the map.
+    @ingroup slam_odometry
+*/
+struct CV_EXPORTS_W_SIMPLE KeyFrame
+{
+    int     id = -1;
+    Matx44d pose_cw; //!< World-to-camera transform (row-major 4x4).
+
+    std::vector<KeyPoint> keypoints;
+    Mat                   descriptors;
+    std::vector<Point2f>  undist_kpts;
+    Size                  imageSize;
+
+    std::vector<int>       kpt_to_mp;  //!< Map-point id per keypoint, -1 if unmatched.
+    std::vector<MapPoint*> mappoints;  //!< Direct pointer per keypoint, nullptr if unmatched.
+
+    std::vector<std::pair<KeyFrame*, int>> ordered_covisibility; //!< Neighbours by shared point count.
+};
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_TYPES_HPP
@@ -0,0 +1,102 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP
+#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP
+
+#include "types.hpp"
+#include "map.hpp"
+#include "odometry_params.hpp"
+#include <opencv2/core.hpp>
+#include <opencv2/features.hpp>
+
+namespace cv { namespace slam {
+
+/** @brief Monocular visual odometry pipeline.
+
+    Use in batch mode (process a whole folder) or frame-by-frame:
+
+    @code
+    auto vo = cv::slam::VisualOdometry::create(
+        detector, matcher, imagesFolder, outputFolder, K, dist, params);
+    vo->run();                  // batch
+    vo->processFrame(img);      // or incremental
+    @endcode
+
+    Output files written by run() to outputFolder:
+    - trajectory.bin  — binary T_cw pose stream
+    - trajectory.txt  — camera centres in world coords
+    - images.txt      — COLMAP-compatible pose file
+    - map_points.txt  — 3-D point cloud
+    - keypoints.txt   — per-keyframe observations
+    - vo.log          — per-frame processing trace
+
+    All poses are world-to-camera (T_cw). Camera centre = -R^T * t.
+
+    @ingroup slam_odometry
+*/
+class CV_EXPORTS_W VisualOdometry : public Algorithm
+{
+public:
+    VisualOdometry();
+    virtual ~VisualOdometry();
+
+    /** @brief Create a VisualOdometry instance.
+        @param detector      Feature detector + descriptor extractor.
+        @param matcher       Descriptor matcher.
+        @param imagesFolder  Input image folder used by run(). May be empty.
+        @param outputFolder  Artifact output folder. May be empty.
+        @param cameraMatrix  3x3 intrinsic matrix K.
+        @param distCoeffs    Distortion coefficients. Pass empty Mat for rectified input.
+        @param params        Odometry parameters.
+    */
+    CV_WRAP static Ptr<VisualOdometry> create(
+        const Ptr<Feature2D>&         detector,
+        const Ptr<DescriptorMatcher>& matcher,
+        const String&                 imagesFolder,
+        const String&                 outputFolder,
+        InputArray                    cameraMatrix,
+        InputArray                    distCoeffs,
+        const OdometryParams&         params = OdometryParams());
+
+    /** @brief Process all images in imagesFolder and write output artifacts.
+        @return true if at least one pose was emitted.
+    */
+    CV_WRAP virtual bool run() = 0;
+
+    /** @brief Process a single frame.
+        @return true if a pose was emitted.
+    */
+    CV_WRAP virtual bool processFrame(InputArray image) = 0;
+
+    /** @brief Reset to NOT_INITIALIZED state and clear the map. */
+    CV_WRAP virtual void reset() = 0;
+
+    CV_WRAP virtual OdometryState                  getState()      const = 0;
+    CV_WRAP virtual Matx44d                        getLastPose()   const = 0;
+    CV_WRAP virtual Map&                           getMap()              = 0;
+    CV_WRAP virtual const std::vector<Matx44d>&   getTrajectory() const = 0;
+
+    CV_WRAP virtual OdometryParams getParams() const = 0;
+    CV_WRAP virtual void           setParams(const OdometryParams& params) = 0;
+
+    /** @brief Enable/disable pose-only BA after each PnP step. No-op without g2o. Default: true. */
+    CV_WRAP virtual void setPoseOptimization(bool enable) = 0;
+    CV_WRAP virtual bool getPoseOptimization() const = 0;
+
+    /** @brief Enable/disable local BA at each keyframe. No-op without g2o. Default: true. */
+    CV_WRAP virtual void setLocalBA(bool enable) = 0;
+    CV_WRAP virtual bool getLocalBA() const = 0;
+
+    CV_WRAP virtual String getImagesFolder() const = 0;
+    CV_WRAP virtual void   setImagesFolder(const String& path) = 0;
+
+    CV_WRAP virtual String getOutputFolder() const = 0;
+    CV_WRAP virtual void   setOutputFolder(const String& path) = 0;
+};
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP
@@ -0,0 +1,8 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#include "perf_precomp.hpp"
+
+CV_PERF_TEST_MAIN(slam)
@@ -0,0 +1,12 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef __OPENCV_PERF_SLAM_PRECOMP_HPP__
+#define __OPENCV_PERF_SLAM_PRECOMP_HPP__
+
+#include <opencv2/ts.hpp>
+#include <opencv2/slam.hpp>
+
+#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__
@@ -0,0 +1,125 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#include "precomp.hpp"
+
+namespace cv { namespace slam {
+
+struct Map::Impl
+{
+    std::vector<KeyFrame*>             kf_vec;
+    std::vector<MapPoint*>             mp_vec;
+    std::unordered_map<int, KeyFrame*> kf_map;
+    std::unordered_map<int, MapPoint*> mp_map;
+    std::vector<Matx44d>               trajectory;
+    int next_kf_id = 0;
+    int next_mp_id = 0;
+};
+
+Map::Map()  : impl_(makePtr<Impl>()) {}
+
+Map::~Map()
+{
+    clear();
+}
+
+int Map::addKeyframe(KeyFrame& kf)
+{
+    if (kf.id < 0)
+        kf.id = impl_->next_kf_id++;
+    KeyFrame* ptr = new KeyFrame(kf);
+    impl_->kf_vec.push_back(ptr);
+    impl_->kf_map[ptr->id] = ptr;
+    kf.id = ptr->id;
+    return ptr->id;
+}
+
+KeyFrame* Map::getKeyframe(int id)
+{
+    auto it = impl_->kf_map.find(id);
+    return it != impl_->kf_map.end() ? it->second : nullptr;
+}
+
+const std::vector<KeyFrame*>& Map::keyframes() const { return impl_->kf_vec; }
+
+int Map::numKeyframes() const { return (int)impl_->kf_vec.size(); }
+
+int Map::addMapPoint(MapPoint& mp)
+{
+    if (mp.id < 0)
+        mp.id = impl_->next_mp_id++;
+    MapPoint* ptr = new MapPoint(mp);
+    impl_->mp_vec.push_back(ptr);
+    impl_->mp_map[ptr->id] = ptr;
+    mp.id = ptr->id;
+    return ptr->id;
+}
+
+MapPoint* Map::getMapPoint(int id)
+{
+    auto it = impl_->mp_map.find(id);
+    return it != impl_->mp_map.end() ? it->second : nullptr;
+}
+
+std::vector<MapPoint*> Map::mapPoints() const
+{
+    std::vector<MapPoint*> live;
+    live.reserve(impl_->mp_vec.size());
+    for (MapPoint* mp : impl_->mp_vec)
+        if (mp && !mp->bad)
+            live.push_back(mp);
+    return live;
+}
+
+int Map::numMapPoints() const
+{
+    int n = 0;
+    for (const MapPoint* mp : impl_->mp_vec)
+        if (mp && !mp->bad) ++n;
+    return n;
+}
+
+void Map::addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp)
+{
+    CV_Assert(kf && mp);
+    mp->observations[kf] = kp_idx;
+    if (kp_idx < (int)kf->mappoints.size())
+        kf->mappoints[kp_idx] = mp;
+    if (kp_idx < (int)kf->kpt_to_mp.size())
+        kf->kpt_to_mp[kp_idx] = mp->id;
+}
+
+void Map::removeMapPoint(int mp_id)
+{
+    MapPoint* mp = getMapPoint(mp_id);
+    if (!mp || mp->bad) return;
+    for (auto& [obs_kf, kp_idx] : mp->observations)
+    {
+        if (!obs_kf) continue;
+        if (kp_idx < (int)obs_kf->mappoints.size()) obs_kf->mappoints[kp_idx] = nullptr;
+        if (kp_idx < (int)obs_kf->kpt_to_mp.size()) obs_kf->kpt_to_mp[kp_idx] = -1;
+    }
+    mp->observations.clear();
+    mp->bad = true;
+}
+
+void Map::appendPose(const Matx44d& T_cw) { impl_->trajectory.push_back(T_cw); }
+
+const std::vector<Matx44d>& Map::trajectory() const { return impl_->trajectory; }
+
+void Map::clear()
+{
+    for (KeyFrame* kf : impl_->kf_vec) delete kf;
+    for (MapPoint* mp : impl_->mp_vec) delete mp;
+    impl_->kf_vec.clear();
+    impl_->mp_vec.clear();
+    impl_->kf_map.clear();
+    impl_->mp_map.clear();
+    impl_->trajectory.clear();
+    impl_->next_kf_id = 0;
+    impl_->next_mp_id = 0;
+}
+
+}} // namespace cv::slam
@@ -0,0 +1,28 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef __OPENCV_SLAM_PRECOMP_HPP__
+#define __OPENCV_SLAM_PRECOMP_HPP__
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/features.hpp>
+#include <opencv2/3d.hpp>
+#include "opencv2/slam.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <map>
+#include <numeric>
+#include <set>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#endif // __OPENCV_SLAM_PRECOMP_HPP__
@@ -0,0 +1,12 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#include "precomp.hpp"
+
+namespace cv { namespace slam {
+
+OdometryParams::OdometryParams() = default;
+
+}} // namespace cv::slam
@@ -0,0 +1,12 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+
+#ifndef __OPENCV_TEST_SLAM_PRECOMP_HPP__
+#define __OPENCV_TEST_SLAM_PRECOMP_HPP__
+
+#include <opencv2/ts.hpp>
+#include <opencv2/slam.hpp>
+
+#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__
@@ -0,0 +1,136 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+// =============================================================================
+// Map tests
+// =============================================================================
+
+TEST(Map, AddAndRetrieveKeyframe)
+{
+    cv::slam::Map map;
+
+    cv::slam::KeyFrame kf;
+    kf.pose_cw = cv::Matx44d::eye();
+
+    int id = map.addKeyframe(kf);
+    EXPECT_GE(id, 0);
+    EXPECT_EQ(kf.id, id);
+
+    cv::slam::KeyFrame* retrieved = map.getKeyframe(id);
+    ASSERT_NE(retrieved, nullptr);
+    EXPECT_EQ(retrieved->id, id);
+    EXPECT_EQ(map.numKeyframes(), 1);
+}
+
+TEST(Map, AddAndRetrieveMapPoint)
+{
+    cv::slam::Map map;
+
+    cv::slam::MapPoint mp;
+    mp.pos = cv::Point3d(1.0, 2.0, 3.0);
+
+    int id = map.addMapPoint(mp);
+    EXPECT_GE(id, 0);
+    EXPECT_EQ(mp.id, id);
+
+    cv::slam::MapPoint* retrieved = map.getMapPoint(id);
+    ASSERT_NE(retrieved, nullptr);
+    EXPECT_DOUBLE_EQ(retrieved->pos.x, 1.0);
+    EXPECT_DOUBLE_EQ(retrieved->pos.y, 2.0);
+    EXPECT_DOUBLE_EQ(retrieved->pos.z, 3.0);
+    EXPECT_FALSE(retrieved->bad);
+    EXPECT_EQ(map.numMapPoints(), 1);
+}
+
+TEST(Map, RemoveMapPointCleansObservations)
+{
+    cv::slam::Map map;
+
+    // Add a keyframe with one keypoint slot.
+    cv::slam::KeyFrame kf;
+    kf.pose_cw  = cv::Matx44d::eye();
+    kf.mappoints.assign(1, nullptr);
+    kf.kpt_to_mp.assign(1, -1);
+    int kf_id = map.addKeyframe(kf);
+
+    // Add a map point.
+    cv::slam::MapPoint mp;
+    mp.pos = cv::Point3d(0, 0, 1);
+    int mp_id = map.addMapPoint(mp);
+
+    cv::slam::KeyFrame* kf_ptr = map.getKeyframe(kf_id);
+    cv::slam::MapPoint* mp_ptr = map.getMapPoint(mp_id);
+    ASSERT_NE(kf_ptr, nullptr);
+    ASSERT_NE(mp_ptr, nullptr);
+
+    // Wire the observation.
+    map.addObservation(kf_ptr, 0, mp_ptr);
+    EXPECT_EQ(mp_ptr->observations.size(), 1u);
+    EXPECT_EQ(kf_ptr->mappoints[0], mp_ptr);
+
+    // Remove the map point and verify cleanup.
+    map.removeMapPoint(mp_id);
+    EXPECT_TRUE(mp_ptr->bad);
+    EXPECT_EQ(kf_ptr->mappoints[0], nullptr);
+    EXPECT_EQ(kf_ptr->kpt_to_mp[0], -1);
+    EXPECT_EQ(map.numMapPoints(), 0);
+}
+
+TEST(Map, TrajectoryAppendAndRetrieve)
+{
+    cv::slam::Map map;
+
+    cv::Matx44d T1 = cv::Matx44d::eye();
+    cv::Matx44d T2 = cv::Matx44d::eye();
+    T2(0, 3) = 1.0;  // 1-unit translation
+
+    map.appendPose(T1);
+    map.appendPose(T2);
+
+    const auto& traj = map.trajectory();
+    ASSERT_EQ(traj.size(), 2u);
+    EXPECT_DOUBLE_EQ(traj[1](0, 3), 1.0);
+}
+
+TEST(Map, ClearResetsState)
+{
+    cv::slam::Map map;
+
+    cv::slam::KeyFrame kf;
+    kf.pose_cw = cv::Matx44d::eye();
+    map.addKeyframe(kf);
+
+    cv::slam::MapPoint mp;
+    mp.pos = cv::Point3d(1, 2, 3);
+    map.addMapPoint(mp);
+
+    map.appendPose(cv::Matx44d::eye());
+    map.clear();
+
+    EXPECT_EQ(map.numKeyframes(), 0);
+    EXPECT_EQ(map.numMapPoints(), 0);
+    EXPECT_TRUE(map.trajectory().empty());
+}
+
+// =============================================================================
+// OdometryParams tests
+// =============================================================================
+
+TEST(OdometryParams, DefaultValues)
+{
+    cv::slam::OdometryParams p;
+    EXPECT_EQ(p.min_init_inliers,           80);
+    EXPECT_DOUBLE_EQ(p.min_init_parallax_deg, 3.0);
+    EXPECT_EQ(p.min_init_points,            50);
+    EXPECT_DOUBLE_EQ(p.hf_ratio_thresh,     0.45);
+    EXPECT_DOUBLE_EQ(p.min_growth_parallax_deg, 1.0);
+    EXPECT_DOUBLE_EQ(p.essential_ransac_thresh, 1.0);
+    EXPECT_DOUBLE_EQ(p.essential_ransac_confidence, 0.999);
+}
+
+}} // namespace opencv_test