|
|
@ -12,12 +12,11 @@ using namespace std; |
|
|
|
|
|
|
|
class Traite_image { |
|
|
|
public: |
|
|
|
const static int SENSITIVITY_VALUE = 35; |
|
|
|
const static int SENSITIVITY_VALUE = 20; |
|
|
|
const static int BLUR_Size = 9; |
|
|
|
const static int CLOSE_SIZE = 20; |
|
|
|
const static int ERODE_SIZE = 5; |
|
|
|
const static int NB_FRAME_DROP = 3; |
|
|
|
constexpr static double CONNECT_MAX_DIST = 40.0; |
|
|
|
const static int CLOSE_SIZE = 30; |
|
|
|
const static int ERODE_SIZE = 2; |
|
|
|
const static int NB_FRAME_DROP = 1; |
|
|
|
|
|
|
|
|
|
|
|
vector<cv::Mat> prevs; |
|
|
@ -200,25 +199,27 @@ class Traite_image { |
|
|
|
//make a bounding rectangle around the largest contour then find its centroid
|
|
|
|
//this will be the object's final esticv::Mated position.
|
|
|
|
vector<cv::Rect> nc_rects; // Non connected rectangles
|
|
|
|
for(int i=0; i<contours.size();i++) |
|
|
|
for(size_t i=0; i<contours.size();i++) |
|
|
|
{ |
|
|
|
nc_rects.push_back(cv::boundingRect(contours[i])); |
|
|
|
//cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
|
|
|
|
} |
|
|
|
|
|
|
|
vector<cv::Rect> c_rects; // Connected rectangles
|
|
|
|
connectBBox(nc_rects, c_rects); |
|
|
|
for (const auto& rect : c_rects) |
|
|
|
cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2); |
|
|
|
|
|
|
|
cv::Rect objBRect = cv::boundingRect(contours.at(contours.size()-1)); |
|
|
|
//cv::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2);
|
|
|
|
papillon::BoundingBox bbox = papillon::BoundingBox(); |
|
|
|
bbox.x = objBRect.x / (float)cur.size().width; |
|
|
|
bbox.y = objBRect.y / (float)cur.size().height; |
|
|
|
bbox.width = objBRect.width / (float)cur.size().width; |
|
|
|
bbox.height = objBRect.height / (float)cur.size().height; |
|
|
|
pub_cmd.publish(bbox); |
|
|
|
cleanBBoxes(nc_rects, cur.size(), c_rects); |
|
|
|
if (c_rects.size() > 0) { |
|
|
|
for (const auto& rect : c_rects) |
|
|
|
cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2); |
|
|
|
|
|
|
|
cv::Rect objBRect = c_rects.front(); |
|
|
|
//cv::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2);
|
|
|
|
papillon::BoundingBox bbox = papillon::BoundingBox(); |
|
|
|
bbox.x = objBRect.x / (float)cur.size().width; |
|
|
|
bbox.y = objBRect.y / (float)cur.size().height; |
|
|
|
bbox.width = objBRect.width / (float)cur.size().width; |
|
|
|
bbox.height = objBRect.height / (float)cur.size().height; |
|
|
|
pub_cmd.publish(bbox); |
|
|
|
} |
|
|
|
} |
|
|
|
//make some temp x and y variables so we dont have to type out so much
|
|
|
|
//~ int x = objectBoundingRectangle.x;
|
|
|
@ -234,47 +235,24 @@ class Traite_image { |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void connectBBox(vector<cv::Rect> nc_rects, vector<cv::Rect> &c_rects) { |
|
|
|
int* blocs_id = new int[nc_rects.size()]; |
|
|
|
int val; |
|
|
|
|
|
|
|
// We find all the bounding boxes that are close enough to each other
|
|
|
|
// And we sort them into teams (blocs_id)
|
|
|
|
for (int i = 0; i < nc_rects.size() - 2; ++i) { |
|
|
|
for (int j = i + 1; j < nc_rects.size() - 1; ++j) { |
|
|
|
if (cv::norm(rectCenter(nc_rects.at(i)) - rectCenter(nc_rects.at(j))) < CONNECT_MAX_DIST) { |
|
|
|
val = min(blocs_id[i], blocs_id[j]); |
|
|
|
blocs_id[j] = blocs_id[i] = val; |
|
|
|
} else if (blocs_id[i] == blocs_id[j]) { |
|
|
|
blocs_id[j] = i + 1; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
int maximum = maxElement(blocs_id,nc_rects.size()) + 1; |
|
|
|
vector<cv::Rect>* sorted_rects = new vector<cv::Rect>[maximum]; |
|
|
|
for (int i = 0; i < maximum; ++i) { |
|
|
|
sorted_rects[i].push_back(nc_rects[i]); |
|
|
|
} |
|
|
|
for (int i = 0; i < maximum; ++i) { |
|
|
|
c_rects.push_back(sorted_rects[i].at(0)); |
|
|
|
for (const auto& rect : sorted_rects[i]) { |
|
|
|
c_rects.back() |= rect; |
|
|
|
} |
|
|
|
} |
|
|
|
void cleanBBoxes(vector<cv::Rect> nc_rects, cv::Size img, vector<cv::Rect> &c_rects) { |
|
|
|
int max = 0; |
|
|
|
for (auto r : nc_rects) { |
|
|
|
cv::Point center = rectCenter(r); |
|
|
|
if (r.width > img.width / 2 || r.height > img.height / 8) |
|
|
|
continue; |
|
|
|
|
|
|
|
delete [] sorted_rects; |
|
|
|
} |
|
|
|
if (center.y > img.height * 2 / 3 || center.y < img.height / 3) |
|
|
|
continue; |
|
|
|
|
|
|
|
int maxElement(int Array[], int array_size) |
|
|
|
{ |
|
|
|
int max = Array[0]; |
|
|
|
for (int i = 1; i < array_size; i++) |
|
|
|
{ |
|
|
|
if (Array[i] > max) |
|
|
|
max = Array[i]; |
|
|
|
int r_area = r.area(); |
|
|
|
if (max < r_area) { |
|
|
|
max = r_area; |
|
|
|
c_rects.insert(c_rects.begin(), r); |
|
|
|
} else { |
|
|
|
c_rects.push_back(r); |
|
|
|
} |
|
|
|
return max; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
cv::Point rectCenter(cv::Rect r) { |
|
|
|