28 #ifndef UTIL3D_MOTION_ESTIMATION_H_
29 #define UTIL3D_MOTION_ESTIMATION_H_
31 #include <rtabmap/core/rtabmap_core_export.h>
43 const std::map<int, cv::Point3f> & words3A,
44 const std::map<int, cv::KeyPoint> & words2B,
45 const CameraModel & cameraModel,
48 double reprojError = 5.,
50 int pnpRefineIterations = 1,
51 int varianceMedianRatio = 4,
52 float maxVariance = 0,
54 const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
55 cv::Mat * covariance = 0,
56 std::vector<int> * matchesOut = 0,
57 std::vector<int> * inliersOut = 0,
58 bool splitLinearCovarianceComponents =
false);
61 const std::map<int, cv::Point3f> & words3A,
62 const std::map<int, cv::KeyPoint> & words2B,
63 const std::vector<CameraModel> & cameraModels,
64 unsigned int samplingPolicy = 0,
67 double reprojError = 5.,
69 int pnpRefineIterations = 1,
70 int varianceMedianRatio = 4,
71 float maxVariance = 0,
73 const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
74 cv::Mat * covariance = 0,
75 std::vector<int> * matchesOut = 0,
76 std::vector<int> * inliersOut = 0,
77 bool splitLinearCovarianceComponents =
false);
80 const std::map<int, cv::Point3f> & words3A,
81 const std::map<int, cv::Point3f> & words3B,
83 double inliersDistance = 0.1,
85 int refineIterations = 5,
86 cv::Mat * covariance = 0,
87 std::vector<int> * matchesOut = 0,
88 std::vector<int> * inliersOut = 0);
91 const std::vector<cv::Point3f> & objectPoints,
92 const std::vector<cv::Point2f> & imagePoints,
93 const cv::Mat & cameraMatrix,
94 const cv::Mat & distCoeffs,
97 bool useExtrinsicGuess,
99 float reprojectionError,
101 std::vector<int> & inliers,
103 int refineIterations = 1,
104 float refineSigma = 3.0f);