435 lines
11 KiB
C++
435 lines
11 KiB
C++
#include "contours_extractor_impl.hpp"
|
|
#include <algorithm>
|
|
#include <cstdlib>
|
|
#include "opencv2/opencv.hpp"
|
|
#include "opencv2/imgproc/imgproc_c.h"
|
|
#include "blob/blob.h"
|
|
#include "blob/BlobDetector.h"
|
|
#include "blob/BlobGroup.h"
|
|
#include "blob/BlobContour.h"
|
|
#include "opencv2/imgproc.hpp"
|
|
#include "bspline.hpp"
|
|
|
|
#define MIN_VALUE 1e-8
|
|
|
|
#define IS_DOUBLE_ZERO(d) (abs(d) < MIN_VALUE)
|
|
|
|
namespace cvpr
|
|
{
|
|
ContoursExtractorImpl::ContoursExtractorImpl()
|
|
{
|
|
|
|
}
|
|
|
|
void ContoursExtractorImpl::init()
|
|
{
|
|
|
|
}
|
|
|
|
void ContoursExtractorImpl::destroy()
|
|
{
|
|
|
|
}
|
|
|
|
void ContoursExtractorImpl::setAreaThreshold(double threshold)
|
|
{
|
|
m_areaThreshold = threshold;
|
|
}
|
|
|
|
double ContoursExtractorImpl::areaThreshold()
|
|
{
|
|
return m_areaThreshold;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setContourOffset(int32_t offset)
|
|
{
|
|
m_contourOffset = offset;
|
|
}
|
|
|
|
int32_t ContoursExtractorImpl::contourOffset()
|
|
{
|
|
return m_contourOffset;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setApproxPolyEpsilon(double epsilon)
|
|
{
|
|
m_approxPolyEpsilon = epsilon;
|
|
}
|
|
|
|
double ContoursExtractorImpl::approxPolyEpsilon()
|
|
{
|
|
return m_approxPolyEpsilon;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setSmoothMethod(SMOOTH_METHOD method)
|
|
{
|
|
m_smoothMethod = method;
|
|
}
|
|
|
|
SMOOTH_METHOD ContoursExtractorImpl::smoothMethod()
|
|
{
|
|
return m_smoothMethod;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setAngelThreshold(double threshold)
|
|
{
|
|
m_angelThreshold = threshold;
|
|
}
|
|
|
|
double ContoursExtractorImpl::angelThreshold()
|
|
{
|
|
return m_angelThreshold;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setDefectThreshold(double threshold)
|
|
{
|
|
m_defectThreshold = threshold;
|
|
}
|
|
|
|
double ContoursExtractorImpl::defectThreshold()
|
|
{
|
|
return m_defectThreshold;
|
|
}
|
|
|
|
void ContoursExtractorImpl::setSmoothStep(double step)
|
|
{
|
|
m_smoothStep = step;
|
|
}
|
|
|
|
double ContoursExtractorImpl::smoothStep()
|
|
{
|
|
return m_smoothStep;
|
|
}
|
|
|
|
void ContoursExtractorImpl::extract(const cv::Mat& alpha, std::vector<std::vector<cv::Point>>& outputContours)
|
|
{
|
|
assert(!alpha.empty());
|
|
if (alpha.empty()) {
|
|
return;
|
|
}
|
|
|
|
cv::Mat edges;
|
|
cv::threshold(alpha, edges, 20, 255, cv::THRESH_BINARY);
|
|
|
|
cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3));
|
|
cv::erode(edges, edges, kernel);
|
|
|
|
if (m_contourOffset == 0) {
|
|
cv::findContours(edges, outputContours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
|
|
return;
|
|
}
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::imshow("edges", edges);
|
|
cv::waitKey(0);
|
|
#endif
|
|
|
|
BlobDetector detector(edges);
|
|
|
|
BlobGroup group = detector.getBlobGroup();
|
|
group.filter(group, FilterAction::FLT_EXCLUDE, BlobGetArea(), FilterCondition::FLT_LESS, m_areaThreshold);
|
|
|
|
cv::Point center(edges.cols / 2, edges.rows / 2);
|
|
Blob* centerBlob = group.getBlobNearestTo(center);
|
|
|
|
cv::Point pt = centerBlob->getCentroid(true);
|
|
for (int i = 0; i < group.getNumBlobs(); i++) {
|
|
auto* blob = group.getBlob(i);
|
|
if (blob != centerBlob) {
|
|
auto center = blob->getCenter();
|
|
cv::line(edges, center, pt, cv::Scalar(255, 255, 255), m_contourOffset, 1);
|
|
}
|
|
}
|
|
|
|
std::vector<std::vector<cv::Point>> contours;
|
|
cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
|
|
|
|
cv::Mat tmp = cv::Mat::zeros(edges.rows, edges.cols, CV_8UC3);
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::drawContours(tmp, contours, 0, cv::Scalar(0, 0, 255), 1);
|
|
#endif
|
|
for (int i = 0; i < contours.size(); ++i) {
|
|
for (int j = 0; j < contours[i].size(); ++j) {
|
|
cv::circle(tmp, contours[i][j], m_contourOffset, cv::Scalar(255, 255, 255), -1);
|
|
}
|
|
}
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::imshow("tmp", tmp);
|
|
cv::waitKey(0);
|
|
#endif
|
|
cv::Mat bin;
|
|
cv::cvtColor(tmp, bin, CV_BGR2GRAY);
|
|
|
|
std::vector<std::vector<cv::Point>> output;
|
|
cv::findContours(bin, output, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::drawContours(tmp, output, 0, cv::Scalar(0, 0, 255), 1);
|
|
cv::imshow("tmp", tmp);
|
|
cv::waitKey(0);
|
|
#endif
|
|
if (m_smoothMethod == SMOOTH_METHOD::BSPLINE) {
|
|
std::vector<cv::Point> approxCurve;
|
|
cv::approxPolyDP(output[0], approxCurve, m_approxPolyEpsilon, false);
|
|
|
|
//output[0].emplace_back(output[0][0]);
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::Mat color = cv::imread("C:/Users/ouxia/source/repos/Contour/data/origin-03.png", cv::ImreadModes::IMREAD_UNCHANGED);
|
|
|
|
std::vector<std::vector<cv::Point >> cs;
|
|
cs.emplace_back(approxCurve);
|
|
cv::drawContours(color, cs, 0, cv::Scalar(255, 255, 255), 1);
|
|
|
|
std::vector<cv::Point> hullPoints;
|
|
cv::convexHull(approxCurve, hullPoints, true, true);
|
|
|
|
std::vector<std::vector<cv::Point >> hullContours;
|
|
hullContours.emplace_back(hullPoints);
|
|
|
|
cv::drawContours(color, hullContours, 0, cv::Scalar(0, 255, 0), 1);
|
|
#endif
|
|
//std::vector<std::vector<int>> indices;
|
|
//getSubCurves(approxCurve, 150.0, indices);
|
|
|
|
//for (const auto& indice : indices) {
|
|
// if (indice.size() < 3) {
|
|
// continue;
|
|
// }
|
|
// std::vector<std::vector<double>> psArr;
|
|
// for (int32_t idx = 0; idx < indice.size(); ++idx) {
|
|
// std::vector<double> pt;
|
|
// pt.emplace_back(approxCurve[indice[idx]].x);
|
|
// pt.emplace_back(approxCurve[indice[idx]].y);
|
|
// psArr.emplace_back(pt);
|
|
// }
|
|
|
|
// BSpline spline(psArr, 2);
|
|
|
|
// for (double t = 0.0; t < 1.0; t += 0.05) {
|
|
// std::vector<double> point = spline.eval(t);
|
|
// if (point.size() > 1) {
|
|
// cv::Point pt;
|
|
// pt.x = point[0];
|
|
// pt.y = point[1];
|
|
|
|
// cv::circle(color, pt, 1, cv::Scalar(0, 0, 255), -1);
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
auto curve = approxCurve;
|
|
|
|
if (!IS_DOUBLE_ZERO(m_defectThreshold)) {
|
|
std::vector<int32_t> hull;
|
|
cv::convexHull(approxCurve, hull, true, false);
|
|
|
|
std::vector<cv::Vec4i> defects;
|
|
cv::convexityDefects(approxCurve, hull, defects);
|
|
|
|
for (const auto& defect : defects) {
|
|
cv::Point startPoint = approxCurve[defect[0]];
|
|
|
|
cv::Point endPoint = approxCurve[defect[1]];
|
|
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::Point farthestPoint = cs[0][defect[2]];
|
|
float depth = defect[3] / 256.0;
|
|
|
|
cv::circle(color, startPoint, 5, cv::Scalar(255, 0, 0), -1);
|
|
cv::circle(color, endPoint, 5, cv::Scalar(255, 127, 0), -1);
|
|
cv::circle(color, farthestPoint, 5, cv::Scalar(0, 255, 255), -1);
|
|
|
|
std::string depthText = "Depth: " + std::to_string(depth);
|
|
cv::putText(color, depthText, farthestPoint + cv::Point(0, -10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
|
|
|
|
cv::imshow("Convexity Defects", color);
|
|
#endif
|
|
int start = -1;
|
|
int end = -1;
|
|
|
|
for (int i = 0; i < curve.size(); ++i) {
|
|
if (startPoint == curve[i]) {
|
|
start = i;
|
|
}
|
|
if (endPoint == curve[i]) {
|
|
end = i;
|
|
}
|
|
}
|
|
|
|
if (start != -1 && end != -1 && start != end) {
|
|
if (euclideanDistance(startPoint, endPoint) < m_defectThreshold) {
|
|
cv::Point connected = (startPoint + endPoint) / 2;
|
|
std::vector<cv::Point> sub{ connected };
|
|
if (start > end) {
|
|
std::swap(start, end);
|
|
}
|
|
replaceSubsequence(curve, start, end, sub);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
curve.emplace_back(curve[0]);
|
|
|
|
std::vector<std::vector<double>> psArr;
|
|
for (int i = 0; i < curve.size(); ++i) {
|
|
std::vector<double> pt;
|
|
pt.emplace_back(curve[i].x);
|
|
pt.emplace_back(curve[i].y);
|
|
psArr.emplace_back(pt);
|
|
}
|
|
|
|
BSpline spline(psArr, 2);
|
|
|
|
if (spline.isValid()) {
|
|
std::vector<double> weights;
|
|
std::vector<cv::Point> points;
|
|
for (double t = 0.0; t < 1.0; t += m_smoothStep) {
|
|
std::vector<double> point = spline.eval(t);
|
|
if (point.size() > 1) {
|
|
cv::Point pt;
|
|
pt.x = point[0];
|
|
pt.y = point[1];
|
|
points.emplace_back(pt);
|
|
}
|
|
}
|
|
|
|
outputContours.emplace_back(points);
|
|
}
|
|
else {
|
|
outputContours = output;
|
|
}
|
|
}
|
|
else {
|
|
outputContours = output;
|
|
}
|
|
#if DEBUG_CONTOURS_EXTRACTOR
|
|
cv::drawContours(alpha, outputContours, 0, cv::Scalar(255, 255, 255), 1);
|
|
cv::imshow("alpha", alpha);
|
|
cv::waitKey(0);
|
|
#endif
|
|
}
|
|
|
|
double ContoursExtractorImpl::calculateAngle(const cv::Point& pt1, const cv::Point& pt2, const cv::Point& pt3)
|
|
{
|
|
cv::Point vec1 = pt1 - pt2;
|
|
cv::Point vec2 = pt3 - pt2;
|
|
|
|
double dotProduct = vec1.x * vec2.x + vec1.y * vec2.y;
|
|
double magnitude1 = std::sqrt(vec1.x * vec1.x + vec1.y * vec1.y);
|
|
double magnitude2 = std::sqrt(vec2.x * vec2.x + vec2.y * vec2.y);
|
|
|
|
double cosTheta = dotProduct / (magnitude1 * magnitude2);
|
|
cosTheta = std::clamp<double>(cosTheta, -1.0, 1.0);
|
|
|
|
return std::acos(cosTheta) * 180.0 / CV_PI;
|
|
}
|
|
|
|
|
|
double ContoursExtractorImpl::euclideanDistance(const cv::Point& p1, const cv::Point& p2)
|
|
{
|
|
return cv::norm(p1 - p2);
|
|
}
|
|
|
|
double ContoursExtractorImpl::curveDistance(const std::vector<cv::Point>& contour, int startIdx, int endIdx)
|
|
{
|
|
double curveDistance = 0.0;
|
|
int n = contour.size();
|
|
|
|
for (int i = startIdx; i != endIdx; i = (i + 1) % n) {
|
|
curveDistance += euclideanDistance(contour[i], contour[(i + 1) % n]);
|
|
}
|
|
|
|
return curveDistance;
|
|
}
|
|
|
|
void ContoursExtractorImpl::getSubCurves(const std::vector<cv::Point>& contour, double thresholdAngle, std::vector<std::vector<int>>& indices)
|
|
{
|
|
std::set<int> currentGroup;
|
|
|
|
for (size_t i = 0; i < contour.size(); ++i) {
|
|
const cv::Point& pt1 = contour[(i + contour.size() - 1) % contour.size()];
|
|
const cv::Point& pt2 = contour[i];
|
|
const cv::Point& pt3 = contour[(i + 1) % contour.size()];
|
|
|
|
double angle = calculateAngle(pt1, pt2, pt3);
|
|
|
|
if (angle < thresholdAngle) {
|
|
currentGroup.insert((i + contour.size() - 1) % contour.size());
|
|
currentGroup.insert(i);
|
|
currentGroup.insert((i + 1) % contour.size());
|
|
}
|
|
else if (!currentGroup.empty()) {
|
|
std::vector<int> vec;
|
|
for (auto it = currentGroup.begin(); it != currentGroup.end(); ++it) {
|
|
vec.emplace_back(*it);
|
|
}
|
|
indices.push_back(vec);
|
|
currentGroup.clear();
|
|
}
|
|
}
|
|
|
|
if (!currentGroup.empty()) {
|
|
std::vector<int> vec;
|
|
for (auto it = currentGroup.begin(); it != currentGroup.end(); ++it) {
|
|
vec.emplace_back(*it);
|
|
}
|
|
indices.push_back(vec);
|
|
}
|
|
}
|
|
|
|
void ContoursExtractorImpl::getOptimalPoints(const std::vector<cv::Point>& contour, int& bestIdx1, int& bestIdx2, double& bestCurveDist, double& bestLineDist)
|
|
{
|
|
bestCurveDist = 0.0;
|
|
bestLineDist = std::numeric_limits<double>::max();
|
|
int n = contour.size();
|
|
|
|
for (int i = 0; i < n; ++i) {
|
|
for (int j = i + 1; j < n; ++j) {
|
|
double curveDist = curveDistance(contour, i, j);
|
|
|
|
double lineDist = euclideanDistance(contour[i], contour[j]);
|
|
|
|
if (curveDist > bestCurveDist || (curveDist == bestCurveDist && lineDist < bestLineDist)) {
|
|
bestCurveDist = curveDist;
|
|
bestLineDist = lineDist;
|
|
bestIdx1 = i;
|
|
bestIdx2 = j;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void ContoursExtractorImpl::replaceSubsequence(std::vector<cv::Point>& contour, int startIdx, int endIdx, const std::vector<cv::Point>& newSubsequence)
|
|
{
|
|
int n = contour.size();
|
|
|
|
if (startIdx < 0 || endIdx < 0 || startIdx >= n || endIdx >= n) {
|
|
return;
|
|
}
|
|
|
|
if (startIdx > endIdx) {
|
|
return;
|
|
}
|
|
|
|
contour.erase(contour.begin() + startIdx, contour.begin() + endIdx + 1);
|
|
|
|
contour.insert(contour.begin() + startIdx, newSubsequence.begin(), newSubsequence.end());
|
|
}
|
|
|
|
cv::Point ContoursExtractorImpl::getPointOnLine(const cv::Point& pt1, const cv::Point& pt2, int32_t x)
|
|
{
|
|
double dx = pt2.x - pt1.x;
|
|
double dy = pt2.y - pt1.y;
|
|
|
|
cv::Point pt;
|
|
pt.x = pt1.x + x * dx;
|
|
pt.y = pt1.y + x * dy;
|
|
|
|
return pt;
|
|
}
|
|
}
|