Rectangle joining test

src/papillon.cpp

@@ -17,6 +17,7 @@ class Traite_image {
 		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;
 
 
 		vector<cv::Mat> prevs;
@@ -198,12 +199,18 @@ class Traite_image {
 				//largestContourVec.push_back(contours.at(contours.size()-1));
 				//make a bounding rectangle around the largest contour then find its centroid
 				//this will be the object's final esticv::Mated position.
-				cv::Rect objectBoundingRectangle;
+				vector<cv::Rect> nc_rects; // Non connected rectangles
 				for(int i=0; i<contours.size();i++)
 				{
-					objectBoundingRectangle = cv::boundingRect(contours[i]);
-					cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
+					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();
@@ -226,6 +233,54 @@ class Traite_image {
 			//putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,cv::Scalar(255,0,0),2);
 		}
 
+
+		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;
+				}
+			}
+
+			delete [] sorted_rects;
+		}
+
+		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];
+				}
+			return max;
+		}
+
+		cv::Point rectCenter(cv::Rect r) {
+			return cv::Point(r.x + r.width/2, r.y + r.height / 2);
+		}
+
 		inline bool isFlowCorrect(cv::Point2f u)
 		{
 			return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;