diff --git a/swri_math_util/include/swri_math_util/ransac.h b/swri_math_util/include/swri_math_util/ransac.h index 101c25c1e..bda7263ac 100644 --- a/swri_math_util/include/swri_math_util/ransac.h +++ b/swri_math_util/include/swri_math_util/ransac.h @@ -49,14 +49,14 @@ namespace swri_math_util explicit Ransac(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {} ModelType FitModel( - const DataType& data, + Model& model, double max_error, double confidence, + int32_t min_iterations, int32_t max_iterations, std::vector& inliers, int32_t& iterations) { - Model model(data); int32_t breakout = std::numeric_limits::max(); ModelType best_fit; inliers.clear(); @@ -75,7 +75,7 @@ namespace swri_math_util std::vector indices; ModelType hypothesis; - for (iterations = 0; iterations < max_iterations && iterations < breakout; iterations++) + for (iterations = 0; (iterations < max_iterations && iterations < breakout) || iterations < min_iterations; iterations++) { indices.clear(); rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices); diff --git a/swri_opencv_util/include/swri_opencv_util/model_fit.h b/swri_opencv_util/include/swri_opencv_util/model_fit.h index deb903f7f..aae87ddd1 100644 --- a/swri_opencv_util/include/swri_opencv_util/model_fit.h +++ b/swri_opencv_util/include/swri_opencv_util/model_fit.h @@ -43,7 +43,7 @@ namespace swri_opencv_util { template cv::Mat FindModel2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -55,26 +55,27 @@ namespace swri_opencv_util swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()) { cv::Mat model; - + // Put data into the expected format. cv::Mat correspondences; if (!ZipCorrespondences(points1, points2, correspondences)) { return model; } - - // Run RANSAC to robustly fit a rigid transform model to the set of + + // Run RANSAC to robustly fit a rigid transform model to the set of // corresponding points. swri_math_util::Ransac ransac(rng); + Model fit_model(correspondences); model = ransac.FitModel( - correspondences, max_error, confidence, max_iterations, good_points, iterations); - + fit_model, max_error, confidence, 1, max_iterations, good_points, iterations); + if (good_points.empty()) { return model; } - + // Populate output data. bool row_order = points1.rows > 1; if (row_order) @@ -97,12 +98,12 @@ namespace swri_opencv_util inliers2.at(0, i) = points2.at(0, good_points[i]); } } - + return model; } cv::Mat FindTranslation2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -114,7 +115,7 @@ namespace swri_opencv_util swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); cv::Mat FindRigidTransform2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -124,12 +125,12 @@ namespace swri_opencv_util double confidence = 0.9, int32_t max_iterations = 1000, swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); - + // Returns a 2x3 transform that can be applied to points1 to align them to points2. cv::Mat FitRigidTransform2d(const cv::Mat& points1, const cv::Mat& points2); - + cv::Mat FindAffineTransform2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -139,11 +140,11 @@ namespace swri_opencv_util double confidence = 0.9, int32_t max_iterations = 1000, swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); - + cv::Mat FitAffineTransform2d(const cv::Mat& points1, const cv::Mat& points2); - + cv::Mat FindHomography( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -153,6 +154,55 @@ namespace swri_opencv_util double confidence = 0.9, int32_t max_iterations = 1000, swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); + + PlaneModel FindPlane( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error = 1.0, + double confidence = 0.9, + int32_t min_iterations = 1, + int32_t max_iterations = 1000, + swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); + + PlaneModel FitPlane(const cv::Mat& points); + + LineModel3d FindLine3d( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error = 1.0, + double confidence = 0.9, + int32_t min_iterations = 1, + int32_t max_iterations = 1000, + swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); + + LineModel3d FindOrthoLine3d( + const cv::Mat& points, + const LineModel3d& ortho, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error = 1.0, + double confidence = 0.9, + int32_t min_iterations = 1, + int32_t max_iterations = 1000, + swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); + + LineModel3d FitLine3d(const cv::Mat& points); + + CrossModel3d FindCross3d( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error = 1.0, + double confidence = 0.9, + int32_t min_iterations = 1, + int32_t max_iterations = 1000, + swri_math_util::RandomGeneratorPtr rng = swri_math_util::RandomGeneratorPtr()); } #endif // OPENCV_UTIL_MODEL_FIT_H_ diff --git a/swri_opencv_util/include/swri_opencv_util/models.h b/swri_opencv_util/include/swri_opencv_util/models.h index 9bc2474b7..a1ab7e577 100644 --- a/swri_opencv_util/include/swri_opencv_util/models.h +++ b/swri_opencv_util/include/swri_opencv_util/models.h @@ -42,25 +42,25 @@ namespace swri_opencv_util public: typedef cv::Mat T; // An Nx4 float matrix typedef cv::Mat M; - + explicit Correspondence2d(const T& data) : data_(data) {} - + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const = 0; int32_t GetInlierCount(const M& model, double max_error); void GetInliers(const M& model, double max_error, std::vector& indices); int32_t Size() const { return data_.rows; } virtual std::string GetModelString(M& model) const { return ""; } - + static void CopyTo(const M& src, M& dst) { src.copyTo(dst); } - + protected: virtual void CalculateNorms(const M& model, cv::Mat& norms); - + const T& data_; - + // Buffer matrices to avoid repeated memory allocations. cv::Mat norms__; cv::Mat predicted__; @@ -73,14 +73,14 @@ namespace swri_opencv_util { public: enum { MIN_SIZE = 4 }; - + explicit Homography(const T& data) : Correspondence2d(data) {} virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; - bool ValidData() const - { - return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; + bool ValidData() const + { + return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; } - + protected: virtual void CalculateNorms(const M& model, cv::Mat& norms); }; @@ -89,12 +89,12 @@ namespace swri_opencv_util { public: enum { MIN_SIZE = 3 }; - + explicit AffineTransform2d(const T& data) : Correspondence2d(data) {} virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; - bool ValidData() const - { - return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; + bool ValidData() const + { + return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; } }; @@ -102,36 +102,202 @@ namespace swri_opencv_util { public: enum { MIN_SIZE = 2 }; - + explicit RigidTransform2d(const T& data) : Correspondence2d(data) {} virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; - bool ValidData() const - { - return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; + bool ValidData() const + { + return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; } }; - + class Translation2d : public Correspondence2d { public: enum { MIN_SIZE = 1 }; - + explicit Translation2d(const T& data) : Correspondence2d(data) {} virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; - bool ValidData() const - { - return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; + bool ValidData() const + { + return data_.cols == 4 && data_.rows >= MIN_SIZE && data_.type() == CV_32F; } }; - + bool Valid2dPointCorrespondences( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2); - + bool ZipCorrespondences( const cv::Mat& points1, const cv::Mat& points2, cv::Mat& correspondences); + + template + class Fit3d + { + public: + typedef cv::Mat T; // An Nx3 float matrix + typedef Model M; // A geometric model + + explicit Fit3d(const T& data) : data_(data) {} + + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const = 0; + int32_t GetInlierCount(const M& model, double max_error) + { + CalculateNorms(model, norms__); + + cv::compare(norms__, cv::Scalar(max_error), thresholded__, cv::CMP_LT); + + return cv::countNonZero(thresholded__); + } + + void GetInliers(const M& model, double max_error, std::vector& indices) + { + CalculateNorms(model, norms__); + + indices.clear(); + indices.reserve(norms__.rows); + double threshold = max_error; + for (int i = 0; i < norms__.rows; i++) + { + if (norms__.at(i) < threshold) + { + indices.push_back(i); + } + } + } + + int32_t Size() const { return data_.rows; } + virtual std::string GetModelString(M& model) const { return ""; } + + static void CopyTo(const M& src, M& dst) + { + src.copyTo(dst); + } + + protected: + virtual void CalculateNorms(const M& model, cv::Mat& norms) = 0; + + const T& data_; + + // Buffer matrices to avoid repeated memory allocations. + cv::Mat norms__; + cv::Mat delta__; + cv::Mat thresholded__; + }; + + struct PlaneModel + { + PlaneModel() : x(0), y(0), z(0), i(0), j(0), k(0) {} + void copyTo(PlaneModel& dst) const + { + dst = *this; + } + + float x, y, z, i, j, k; + }; + + class PlaneFit : public Fit3d + { + public: + enum { MIN_SIZE = 3 }; + + PlaneFit(const T& data, float min_angle = 0.2) : + Fit3d(data), + min_angle_(min_angle) {} + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; + bool ValidData() const + { + return data_.cols == 1 && data_.rows >= MIN_SIZE && data_.type() == CV_32FC3; + } + + protected: + virtual void CalculateNorms(const M& model, cv::Mat& norms); + + float min_angle_; + }; + + struct LineModel3d + { + LineModel3d() : x(0), y(0), z(0), i(0), j(0), k(0) {} + void copyTo(LineModel3d& dst) const + { + dst = *this; + } + + float x, y, z, i, j, k; + }; + + class LineFit3d : public Fit3d + { + public: + enum { MIN_SIZE = 2 }; + + LineFit3d(const T& data) : Fit3d(data) {} + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; + bool ValidData() const + { + return data_.cols == 1 && data_.rows >= MIN_SIZE && data_.type() == CV_32FC3; + } + + protected: + virtual void CalculateNorms(const M& model, cv::Mat& norms); + + cv::Mat temp1__; + cv::Mat temp2__; + cv::Mat x0_p_dot_n__; + cv::Mat x0_p__; + }; + + class OrthoLineFit3d : public LineFit3d + { + public: + OrthoLineFit3d(const T& data, const LineModel3d& ortho, float angle_tolerance = 0.09) : + LineFit3d(data), ortho_(ortho), angle_tolerance_(angle_tolerance) {} + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; + + protected: + LineModel3d ortho_; + float angle_tolerance_; + }; + + struct CrossModel3d + { + CrossModel3d() : x(0), y(0), z(0), i1(0), j1(0), k1(0), i2(0), j2(0), k2(0) {} + void copyTo(CrossModel3d& dst) const + { + dst = *this; + } + + float x, y, z, i1, j1, k1, i2, j2, k2; + }; + + class CrossFit3d : public Fit3d + { + public: + enum { MIN_SIZE = 3 }; + + CrossFit3d(const T& data, float min_angle = 0.2) : + Fit3d(data), + min_angle_(min_angle) {} + virtual bool GetModel(const std::vector& indices, M& model, double max_error) const; + bool ValidData() const + { + return data_.cols == 1 && data_.rows >= MIN_SIZE && data_.type() == CV_32FC3; + } + + protected: + virtual void CalculateNorms(const M& model, cv::Mat& norms); + + float min_angle_; + cv::Mat temp1__; + cv::Mat temp2__; + cv::Mat temp3__; + cv::Mat x0_p_dot_n__; + cv::Mat x0_p__; + }; + } #endif // OPENCV_UTIL_MODELS_H_ diff --git a/swri_opencv_util/src/model_fit.cpp b/swri_opencv_util/src/model_fit.cpp index 6ddea8d95..4c774b233 100644 --- a/swri_opencv_util/src/model_fit.cpp +++ b/swri_opencv_util/src/model_fit.cpp @@ -29,10 +29,14 @@ #include +#include + +#include + namespace swri_opencv_util { cv::Mat FindTranslation2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -49,7 +53,7 @@ namespace swri_opencv_util } cv::Mat FindRigidTransform2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -64,35 +68,35 @@ namespace swri_opencv_util points1, points2, inliers1, inliers2, good_points, iterations, max_error, confidence, max_iterations, rng); } - + cv::Mat FitRigidTransform2d(const cv::Mat& points1, const cv::Mat& points2) { cv::Mat transform; - + if (!Valid2dPointCorrespondences(points1, points2)) { return transform; } - - // The Kabsch algorithm, for calculating the optimal rotation matrix that + + // The Kabsch algorithm, for calculating the optimal rotation matrix that // minimizes the RMSD (root mean squared deviation) between two paired sets // of points. http://en.wikipedia.org/wiki/Kabsch_algorithm - + // Get the centroids of the points. cv::Scalar centroid1 = cv::mean(points1); cv::Scalar centroid2 = cv::mean(points2); - + // Center the points around the origin. cv::Mat points1_centered = (points1 - centroid1); cv::Mat points2_centered = (points2 - centroid2); - + // Reshape the points into 2xN matrices. points1_centered = points1_centered.reshape(1, points1.rows); points2_centered = points2_centered.reshape(1, points1.rows); - + // Compute the covariance matrix. cv::Mat cov = points1_centered.t() * points2_centered; - + // Compute the optimal rotation matrix. cv::Mat W, U, Vt; cv::SVD::compute(cov, W, U, Vt); @@ -100,7 +104,7 @@ namespace swri_opencv_util cv::Mat I = cv::Mat::eye(2, 2, CV_32F); I.at(1, 1) = d; cv::Mat rotation = Vt.t() * I * U.t(); - + // Compute the optimal translation. cv::Mat c1_r(2, 1, CV_32F); c1_r.at(0, 0) = centroid1[0]; @@ -108,7 +112,7 @@ namespace swri_opencv_util c1_r = rotation * c1_r; float t_x = centroid2[0] - c1_r.at(0, 0); float t_y = centroid2[1] - c1_r.at(1, 0); - + transform.create(2, 3, CV_32F); transform.at(0, 0) = rotation.at(0, 0); transform.at(0, 1) = rotation.at(0, 1); @@ -116,12 +120,12 @@ namespace swri_opencv_util transform.at(1, 1) = rotation.at(1, 1); transform.at(0, 2) = t_x; transform.at(1, 2) = t_y; - + return transform; } - + cv::Mat FindAffineTransform2d( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -138,7 +142,7 @@ namespace swri_opencv_util } cv::Mat FindHomography( - const cv::Mat& points1, + const cv::Mat& points1, const cv::Mat& points2, cv::Mat& inliers1, cv::Mat& inliers2, @@ -157,15 +161,15 @@ namespace swri_opencv_util cv::Mat FitAffineTransform2d(const cv::Mat& points1, const cv::Mat& points2) { cv::Mat transform; - + if (!Valid2dPointCorrespondences(points1, points2)) { return transform; } - + bool row_order = points1.rows > 1; int32_t size = row_order ? points1.rows : points1.cols; - + // Perform least squares fit on inliers to refine model. // For least squares there are several decomposition methods: // DECOMP_LU @@ -190,7 +194,7 @@ namespace swri_opencv_util { B = points2.t(); B = B.reshape(1, 2); - + for (int32_t i = 0; i < size; ++i) { const cv::Vec2f& point = points1.at(0, i); @@ -200,7 +204,7 @@ namespace swri_opencv_util A_i[2] = 1.0; } } - + cv::Mat x; if (cv::solve(A, B, x)) { @@ -209,4 +213,201 @@ namespace swri_opencv_util return transform; } + + PlaneModel FindPlane( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error, + double confidence, + int32_t min_iterations, + int32_t max_iterations, + swri_math_util::RandomGeneratorPtr rng) + { + swri_math_util::Ransac ransac(rng); + + cv::Mat reshaped = points.reshape(3); + PlaneFit fit_model(reshaped); + PlaneModel model = ransac.FitModel( + fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations); + + if (good_points.empty()) + { + return model; + } + + inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type()); + for (size_t i = 0; i < good_points.size(); ++i) + { + inliers.at(i, 0) = reshaped.at(good_points[i], 0); + } + inliers.reshape(points.channels()); + return model; + } + + PlaneModel FitPlane(const cv::Mat& points) + { + PlaneModel model; + if (points.rows < 3) + { + return model; + } + cv::Mat centroid; + cv::reduce(points.reshape(3), centroid, 0, CV_REDUCE_AVG); + + cv::Scalar c(centroid.at(0, 0), centroid.at(0, 1), centroid.at(0, 2)); + + cv::Mat A; + cv::subtract(points, c, A); + + cv::SVD svd; + cv::Mat w, u, vt; + cv::Mat At; + cv::transpose(A.reshape(1), At); + svd.compute(At, w, u, vt); + + cv::Point min_w_loc; + cv::minMaxLoc(w, NULL, NULL, &min_w_loc); + + model.x = centroid.at(0, 0); + model.y = centroid.at(0, 1); + model.z = centroid.at(0, 2); + model.i = u.at(0, min_w_loc.y); + model.j = u.at(1, min_w_loc.y); + model.k = u.at(2, min_w_loc.y); + + return model; + } + + LineModel3d FindLine3d( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error, + double confidence, + int32_t min_iterations, + int32_t max_iterations, + swri_math_util::RandomGeneratorPtr rng) + { + swri_math_util::Ransac ransac(rng); + + cv::Mat reshaped = points.reshape(3); + LineFit3d fit_model(reshaped); + LineModel3d model = ransac.FitModel( + fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations); + + if (good_points.empty()) + { + return model; + } + + inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type()); + for (size_t i = 0; i < good_points.size(); ++i) + { + inliers.at(i, 0) = reshaped.at(good_points[i], 0); + } + inliers.reshape(points.channels()); + return model; + } + + LineModel3d FindOrthoLine3d( + const cv::Mat& points, + const LineModel3d& ortho, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error, + double confidence, + int32_t min_iterations, + int32_t max_iterations, + swri_math_util::RandomGeneratorPtr rng) + { + swri_math_util::Ransac ransac(rng); + + cv::Mat reshaped = points.reshape(3); + OrthoLineFit3d fit_model(reshaped, ortho); + LineModel3d model = ransac.FitModel( + fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations); + + if (good_points.empty()) + { + return model; + } + + inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type()); + for (size_t i = 0; i < good_points.size(); ++i) + { + inliers.at(i, 0) = reshaped.at(good_points[i], 0); + } + inliers.reshape(points.channels()); + return model; + } + + LineModel3d FitLine3d(const cv::Mat& points) + { + LineModel3d model; + if (points.rows < 2) + { + return model; + } + cv::Mat centroid; + cv::reduce(points.reshape(3), centroid, 0, CV_REDUCE_AVG); + + cv::Scalar c(centroid.at(0, 0), centroid.at(0, 1), centroid.at(0, 2)); + + cv::Mat A; + cv::subtract(points, c, A); + + cv::SVD svd; + cv::Mat w, u, vt; + cv::Mat At; + cv::transpose(A.reshape(1), At); + svd.compute(At, w, u, vt); + + cv::Point max_w_loc; + cv::minMaxLoc(w, NULL, NULL, NULL, &max_w_loc); + + model.x = centroid.at(0, 0); + model.y = centroid.at(0, 1); + model.z = centroid.at(0, 2); + model.i = u.at(0, max_w_loc.y); + model.j = u.at(1, max_w_loc.y); + model.k = u.at(2, max_w_loc.y); + + return model; + } + + CrossModel3d FindCross3d( + const cv::Mat& points, + cv::Mat& inliers, + std::vector &good_points, + int32_t& iterations, + double max_error, + double confidence, + int32_t min_iterations, + int32_t max_iterations, + swri_math_util::RandomGeneratorPtr rng) + { + swri_math_util::Ransac ransac(rng); + + cv::Mat reshaped = points.reshape(3); + CrossFit3d fit_model(reshaped); + CrossModel3d model = ransac.FitModel( + fit_model, max_error, confidence, min_iterations, max_iterations, good_points, iterations); + + if (good_points.empty()) + { + return model; + } + + inliers = cv::Mat(good_points.size(), reshaped.cols, reshaped.type()); + for (size_t i = 0; i < good_points.size(); ++i) + { + inliers.at(i, 0) = reshaped.at(good_points[i], 0); + } + inliers.reshape(points.channels()); + return model; + } } diff --git a/swri_opencv_util/src/models.cpp b/swri_opencv_util/src/models.cpp index 50bc9c374..0979556c2 100644 --- a/swri_opencv_util/src/models.cpp +++ b/swri_opencv_util/src/models.cpp @@ -42,7 +42,7 @@ namespace swri_opencv_util return cv::countNonZero(thresholded__); } - + void Correspondence2d::GetInliers(const M& model, double max_error, std::vector& indices) { CalculateNorms(model, norms__); @@ -67,7 +67,7 @@ namespace swri_opencv_util cv::subtract(predicted__.reshape(1), measured, delta__); cv::multiply(delta__, delta__, delta_squared__); cv::add( - delta_squared__(cv::Rect(0, 0, 1, delta__.rows)), + delta_squared__(cv::Rect(0, 0, 1, delta__.rows)), delta_squared__(cv::Rect(1, 0, 1, delta__.rows)), norms); } @@ -78,19 +78,19 @@ namespace swri_opencv_util { return false; } - + cv::Mat src(MIN_SIZE, 1, CV_32FC2); cv::Mat dst(MIN_SIZE, 1, CV_32FC2); - + for (int32_t i = 0; i < MIN_SIZE; i++) { const float* sample = data_.ptr(indices[i]); src.at(i, 0) = cv::Vec2f(sample[0], sample[1]); dst.at(i, 0) = cv::Vec2f(sample[2], sample[3]); } - + model = cv::getPerspectiveTransform(src, dst); - + // Test input points for if they all match the generated model. cv::Mat predicted; cv::perspectiveTransform(src, predicted, model); @@ -98,16 +98,16 @@ namespace swri_opencv_util cv::subtract(predicted.reshape(1), dst.reshape(1), delta); cv::multiply(delta, delta, delta_squared); cv::add( - delta_squared(cv::Rect(0, 0, 1, delta.rows)), + delta_squared(cv::Rect(0, 0, 1, delta.rows)), delta_squared(cv::Rect(1, 0, 1, delta.rows)), norms); - + double min, max; cv::minMaxLoc(norms, &min, &max); - + return max < max_error * max_error; } - + void Homography::CalculateNorms(const M& model, cv::Mat& norms) { cv::Mat src = data_(cv::Rect(0, 0, 2, data_.rows)).reshape(2); @@ -116,30 +116,30 @@ namespace swri_opencv_util cv::subtract(predicted__.reshape(1), measured, delta__); cv::multiply(delta__, delta__, delta_squared__); cv::add( - delta_squared__(cv::Rect(0, 0, 1, delta__.rows)), + delta_squared__(cv::Rect(0, 0, 1, delta__.rows)), delta_squared__(cv::Rect(1, 0, 1, delta__.rows)), norms); } - + bool AffineTransform2d::GetModel(const std::vector& indices, M& model, double max_error) const { if (indices.size() != MIN_SIZE) { return false; } - + cv::Mat src(MIN_SIZE, 1, CV_32FC2); cv::Mat dst(MIN_SIZE, 1, CV_32FC2); - + for (int32_t i = 0; i < MIN_SIZE; i++) { const float* sample = data_.ptr(indices[i]); src.at(i, 0) = cv::Vec2f(sample[0], sample[1]); dst.at(i, 0) = cv::Vec2f(sample[2], sample[3]); } - + model = cv::getAffineTransform(src, dst); - + // Test input points for if they all match the generated model. cv::Mat predicted; cv::transform(src, predicted, model); @@ -147,13 +147,13 @@ namespace swri_opencv_util cv::subtract(predicted.reshape(1), dst.reshape(1), delta); cv::multiply(delta, delta, delta_squared); cv::add( - delta_squared(cv::Rect(0, 0, 1, delta.rows)), + delta_squared(cv::Rect(0, 0, 1, delta.rows)), delta_squared(cv::Rect(1, 0, 1, delta.rows)), norms); - + double min, max; cv::minMaxLoc(norms, &min, &max); - + return max < max_error * max_error; } @@ -163,10 +163,10 @@ namespace swri_opencv_util { return false; } - + cv::Point2f src[MIN_SIZE]; cv::Point2f dst[MIN_SIZE]; - + for (int32_t i = 0; i < MIN_SIZE; i++) { const float* sample = data_.ptr(indices[i]); @@ -175,43 +175,43 @@ namespace swri_opencv_util dst[i].x = sample[2]; dst[i].y = sample[3]; } - + double len_src = cv::norm(src[1] - src[0]); double len_dst = cv::norm(dst[1] - dst[0]); if (std::fabs(len_src - len_dst) >= max_error) { return false; } - + // Construct a x-axis for both sets. cv::Point2f src_x = (src[1] - src[0]) * (1.0 / len_src); cv::Point2f dst_x = (dst[1] - dst[0]) * (1.0 / len_dst); - + // Construct a y-axis for both sets. cv::Point2f src_y(src_x.y, -src_x.x); src_y *= 1.0 / cv::norm(src_y); - + cv::Point2f dst_y(dst_x.y, -dst_x.x); dst_y *= 1.0 / cv::norm(dst_y); - + // Build rotation matrices for both sets. cv::Mat src_r(2, 2, CV_32F); src_r.at(0, 0) = src_x.x; src_r.at(1, 0) = src_x.y; src_r.at(0, 1) = src_y.x; src_r.at(1, 1) = src_y.y; - + cv::Mat dst_r(2, 2, CV_32F); dst_r.at(0, 0) = dst_x.x; dst_r.at(1, 0) = dst_x.y; dst_r.at(0, 1) = dst_y.x; dst_r.at(1, 1) = dst_y.y; - + // Solve for the rotation between src and dst // R R_src = R_dst // R = R_dst R_src^T cv::Mat rotation = dst_r * src_r.t(); - + // Calculate the translation between src (rotated) and dst. cv::Mat src0_rotated(2, 1, CV_32F); src0_rotated.at(0, 0) = src[0].x; @@ -219,7 +219,7 @@ namespace swri_opencv_util src0_rotated = rotation * src0_rotated; float t_x = dst[0].x - src0_rotated.at(0, 0); float t_y = dst[0].y - src0_rotated.at(1, 0); - + model.create(2, 3, CV_32F); model.at(0, 0) = rotation.at(0, 0); model.at(0, 1) = rotation.at(0, 1); @@ -227,30 +227,30 @@ namespace swri_opencv_util model.at(1, 1) = rotation.at(1, 1); model.at(0, 2) = t_x; model.at(1, 2) = t_y; - + return true; } - + bool Translation2d::GetModel(const std::vector& indices, M& model, double max_error) const { if (indices.size() != MIN_SIZE) { return false; } - + cv::Point2f src; cv::Point2f dst; - + const float* sample = data_.ptr(indices[0]); src.x = sample[0]; src.y = sample[1]; dst.x = sample[2]; dst.y = sample[3]; - + // Calculate the translation between src (rotated) and dst. float t_x = dst.x - src.x; float t_y = dst.y - src.y; - + model.create(2, 3, CV_32F); model.at(0, 0) = 1.0f; model.at(0, 1) = 0.0f; @@ -258,7 +258,7 @@ namespace swri_opencv_util model.at(1, 1) = 1.0f; model.at(0, 2) = t_x; model.at(1, 2) = t_y; - + return true; } @@ -270,35 +270,35 @@ namespace swri_opencv_util { return false; } - + if (points1.type() != CV_32FC2) { return false; } - + if (points1.cols != points2.cols || points1.rows != points2.rows) { return false; } - + if (points1.cols != 1 && points1.rows != 1) { return false; } - + return true; } - + bool ZipCorrespondences( const cv::Mat& points1, const cv::Mat& points2, cv::Mat& correspondeces) - { + { if (!Valid2dPointCorrespondences(points1, points2)) { return false; } - + size_t num_points = points1.cols; bool row_order = false; if (points1.rows > 1) @@ -306,7 +306,7 @@ namespace swri_opencv_util row_order = true; num_points = points1.rows; } - + // Put data into the correct format. if (row_order) { @@ -315,11 +315,316 @@ namespace swri_opencv_util else { cv::hconcat( - points1.reshape(0, num_points).reshape(1), + points1.reshape(0, num_points).reshape(1), points2.reshape(0, num_points).reshape(1), correspondeces); } - + + return true; + } + + + bool PlaneFit::GetModel(const std::vector& indices, M& model, double max_error) const + { + if (indices.size() != MIN_SIZE) + { + return false; + } + + // Check if points are collinear. + + cv::Mat points = data_.reshape(3); + + cv::Vec3f p1 = points.at(indices[0], 0); + cv::Vec3f p2 = points.at(indices[1], 0); + cv::Vec3f p3 = points.at(indices[2], 0); + + cv::Point3f v12 = p2 - p1; + cv::Point3f v13 = p3 - p1; + float d12 = cv::norm(v12); + float d13 = cv::norm(v13); + float d = std::fabs(d12 * d13); + if (d == 0) + { + return false; + } + + float angle = std::acos(v12.dot(v13) / d); + if (angle < min_angle_ || angle + min_angle_ > M_PI) + { + return false; + } + + // Calculate model. + + cv::Vec3f normal = v12.cross(v13); + normal = normal / cv::norm(normal); + + model.x = p1[0]; + model.y = p1[1]; + model.z = p1[2]; + model.i = normal[0]; + model.j = normal[1]; + model.k = normal[2]; + + return true; + } + + void PlaneFit::CalculateNorms(const M& model, cv::Mat& norms) + { + // Subtract the origin point of the plane from each data point + cv::Mat single_column = data_.reshape(3); + cv::subtract(single_column, cv::Scalar(model.x, model.y, model.z), delta__); + + // Calculate the dot product of each adjusted data point with the normal + // of the plane. + cv::Mat split_columns = delta__.reshape(1); + cv::Mat x = split_columns(cv::Rect(0, 0, 1, split_columns.rows)); + cv::Mat y = split_columns(cv::Rect(1, 0, 1, split_columns.rows)); + cv::Mat z = split_columns(cv::Rect(2, 0, 1, split_columns.rows)); + x = x * model.i; + y = y * model.j; + z = z * model.k; + cv::add(x, y, norms); + cv::add(norms, z, norms); + norms = cv::abs(norms); + } + + bool LineFit3d::GetModel(const std::vector& indices, M& model, double max_error) const + { + if (indices.size() != MIN_SIZE) + { + return false; + } + + cv::Mat points = data_.reshape(3); + + cv::Point3f p1(points.at(indices[0], 0)); + cv::Point3f p2(points.at(indices[1], 0)); + + cv::Point3f v12 = p2 - p1; + v12 = cv::Vec3f(v12) / cv::norm(v12); + + model.x = p1.x; + model.y = p1.y; + model.z = p1.z; + model.i = v12.x; + model.j = v12.y; + model.k = v12.z; + + return true; + } + + void LineFit3d::CalculateNorms(const M& model, cv::Mat& norms) + { + // d = ||(x0 - p) - ((x0 - p) . n)n|| + + cv::Scalar x0(model.x, model.y, model.z); + cv::Scalar n(model.i, model.j, model.k); + cv::Mat p = data_.reshape(3); + + if (temp1__.size != p.size) + { + temp1__ = cv::Mat(p.rows, p.cols, p.type()); + } + temp1__.setTo(n); + cv::Mat n_columns = temp1__.reshape(1); + cv::Mat n_x = n_columns(cv::Rect(0, 0, 1, n_columns.rows)); + cv::Mat n_y = n_columns(cv::Rect(1, 0, 1, n_columns.rows)); + cv::Mat n_z = n_columns(cv::Rect(2, 0, 1, n_columns.rows)); + + // (x0 - p) + cv::subtract(p, x0, x0_p__); + + // (x0 - p) . n + cv::multiply(x0_p__, temp1__, temp2__); + cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, CV_REDUCE_SUM); + + // ((x0 - p) . n)n + cv::multiply(n_x, x0_p_dot_n__, n_x); + cv::multiply(n_y, x0_p_dot_n__, n_y); + cv::multiply(n_z, x0_p_dot_n__, n_z); + + // (x0 - p) - ((x0 - p) . n)n + cv::subtract(x0_p__, temp1__, temp1__); + + // d = ||(x0 - p) - ((x0 - p) . n)n|| + cv::multiply(n_x, n_x, n_x); + cv::multiply(n_y, n_y, n_y); + cv::multiply(n_z, n_z, n_z); + cv::reduce(temp1__.reshape(1), norms, 1, CV_REDUCE_SUM); + cv::sqrt(norms, norms); + } + + bool OrthoLineFit3d::GetModel(const std::vector& indices, M& model, double max_error) const + { + if (indices.size() != MIN_SIZE) + { + return false; + } + + cv::Mat points = data_.reshape(3); + + cv::Point3f p1(points.at(indices[0], 0)); + cv::Point3f p2(points.at(indices[1], 0)); + + cv::Point3f v12 = p2 - p1; + v12 = cv::Vec3f(v12) / cv::norm(v12); + + // Check if orthogonal + cv::Point3f ortho(ortho_.i, ortho_.j, ortho_.k); + ortho = cv::Vec3f(ortho) / cv::norm(ortho); + + float angle = std::acos(v12.dot(ortho)); + float error = std::fabs(M_PI * 0.5 - angle); + if (error > angle_tolerance_) + { + return false; + } + + model.x = p1.x; + model.y = p1.y; + model.z = p1.z; + model.i = v12.x; + model.j = v12.y; + model.k = v12.z; + + return true; + } + + bool CrossFit3d::GetModel(const std::vector& indices, M& model, double max_error) const + { + if (indices.size() != MIN_SIZE) + { + return false; + } + + // Check if points are collinear. + + cv::Mat points = data_.reshape(3); + + cv::Point3f p1(points.at(indices[0], 0)); + cv::Point3f p2(points.at(indices[1], 0)); + cv::Point3f p3(points.at(indices[2], 0)); + + cv::Point3f v12 = p2 - p1; + cv::Point3f v13 = p3 - p1; + float d12 = cv::norm(v12); + float d13 = cv::norm(v13); + float d = std::fabs(d12 * d13); + if (d == 0) + { + return false; + } + + float angle = std::acos(v12.dot(v13) / d); + if (angle < min_angle_ || angle + min_angle_ > M_PI) + { + return false; + } + + // Calculate model. + + // Orthogonally project 3rd point onto first line to + // get line through 3rd point orthogonal to the first + // line. + cv::Point3f p4 = p1 + (v13.dot(v12) / v12.dot(v12)) * v12; + cv::Point3f v34 = p4 - p3; + + // Get unit vector of the lines. + v12 = cv::Vec3f(v12) / cv::norm(v12); + v34 = cv::Vec3f(v34) / cv::norm(v34); + + model.x = p4.x; + model.y = p4.y; + model.z = p4.z; + model.i1 = v12.x; + model.j1 = v12.y; + model.k1 = v12.z; + model.i2 = v34.x; + model.j2 = v34.y; + model.k2 = v34.z; + return true; } + + void CrossFit3d::CalculateNorms(const M& model, cv::Mat& norms) + { + ////////////////////////////////////////// + // Line 1 + ////////////////////////////////////////// + + // d = ||(x0 - p) - ((x0 - p) . n)n|| + + cv::Scalar x0(model.x, model.y, model.z); + cv::Scalar n(model.i1, model.j1, model.k1); + cv::Mat p = data_.reshape(3); + + if (temp1__.size != p.size) + { + temp1__ = cv::Mat(p.rows, p.cols, p.type()); + } + temp1__.setTo(n); + cv::Mat n_columns = temp1__.reshape(1); + cv::Mat n_x = n_columns(cv::Rect(0, 0, 1, n_columns.rows)); + cv::Mat n_y = n_columns(cv::Rect(1, 0, 1, n_columns.rows)); + cv::Mat n_z = n_columns(cv::Rect(2, 0, 1, n_columns.rows)); + + // (x0 - p) + cv::subtract(p, x0, x0_p__); + + // (x0 - p) . n + cv::multiply(x0_p__, temp1__, temp2__); + cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, CV_REDUCE_SUM); + + // ((x0 - p) . n)n + cv::multiply(n_x, x0_p_dot_n__, n_x); + cv::multiply(n_y, x0_p_dot_n__, n_y); + cv::multiply(n_z, x0_p_dot_n__, n_z); + + // (x0 - p) - ((x0 - p) . n)n + cv::subtract(x0_p__, temp1__, temp1__); + + // d = ||(x0 - p) - ((x0 - p) . n)n|| + cv::multiply(n_x, n_x, n_x); + cv::multiply(n_y, n_y, n_y); + cv::multiply(n_z, n_z, n_z); + cv::reduce(temp1__.reshape(1), temp3__, 1, CV_REDUCE_SUM); + cv::sqrt(temp3__, temp3__); + + + ////////////////////////////////////////// + // Line 2 + ////////////////////////////////////////// + + // d = ||(x0 - p) - ((x0 - p) . n)n|| + + n = cv::Scalar(model.i2, model.j2, model.k2); + temp1__.setTo(n); + + // (x0 - p) + cv::subtract(p, x0, x0_p__); + + // (x0 - p) . n + cv::multiply(x0_p__, temp1__, temp2__); + cv::reduce(temp2__.reshape(1), x0_p_dot_n__, 1, CV_REDUCE_SUM); + + // ((x0 - p) . n)n + cv::multiply(n_x, x0_p_dot_n__, n_x); + cv::multiply(n_y, x0_p_dot_n__, n_y); + cv::multiply(n_z, x0_p_dot_n__, n_z); + + // (x0 - p) - ((x0 - p) . n)n + cv::subtract(x0_p__, temp1__, temp1__); + + // d = ||(x0 - p) - ((x0 - p) . n)n|| + cv::multiply(n_x, n_x, n_x); + cv::multiply(n_y, n_y, n_y); + cv::multiply(n_z, n_z, n_z); + cv::reduce(temp1__.reshape(1), norms, 1, CV_REDUCE_SUM); + cv::sqrt(norms, norms); + + // Use the minimum distance to either line. + norms = cv::min(norms, temp3__); + } }