I think this fix works to better use LED inference

CompleteVision_MAIN/CompleteVision_MAIN.depend

@@ -152,7 +152,7 @@
 	<vector>
 	<string>
 
-1386715088 source:/home/mdarling/Desktop/CompleteVision_MAIN/PnPObj.cpp
+1386967819 source:/home/mdarling/Desktop/CompleteVision_MAIN/PnPObj.cpp
 	"PnPObj.hpp"
 
 1381786430 source:/home/mdarling/Desktop/CompleteVision_MAIN/serialCommBeagle.cpp
@@ -263,17 +263,17 @@
 	<sys/stat.h>
 	<ctime>
 
-1386805576 source:/home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.cpp
+1386968734 source:/home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.cpp
 	"LED_Detector.hpp"
 
-1386818986 /home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.hpp
+1386830602 /home/mdarling/Desktop/CompleteVision_MAIN/LED_Detector.hpp
 	"Global.hpp"
 	<opencv2/opencv.hpp>
 	<vector>
 	<algorithm>
 	<stdio.h>
 
-1386738559 source:/home/mdarling/Desktop/CompleteVision_MAIN/FeatureDetector_Test.cpp
+1386967558 source:/home/mdarling/Desktop/CompleteVision_MAIN/FeatureDetector_Test.cpp
 	"Global.hpp"
 	"Config.hpp"
 	"v4l2_c.h"

FeatureDetector_Test.cpp

@@ -214,7 +214,7 @@ while(1)
         Detector.findLEDs(frame,gray,binary,imagePoints,DetectorParams,PnP.is_current,PnP.projImagePoints);
 
     // Compute pose estimate
-    int poseIters = PnP.localizeUAV(imagePoints, poseState, poseErr, 9, POSE_ERR_TOL, SECONDARY_POSE_ERR_TOL); //, preCorrelated);
+    int poseIters = PnP.localizeUAV(imagePoints, poseState, poseErr, 9, POSE_ERR_TOL, SECONDARY_POSE_ERR_TOL,preCorrelated);
     if ( poseIters > 0 && checkSanity(poseState) > 0 )
     {
             PnP.is_current = true;
@@ -238,7 +238,7 @@ while(1)
     /// Print fps and pose estimate to console in real-time
 #ifdef POSE_TO_CONSOLE
     double fps_cnt=fps.fps();
-    printf("\e[J  FPS:  %6.2f        # of detected features: %4d\n",fps_cnt,imagePoints.size());
+    printf("\e[J  FPS:  %6.2f        # of detected features: %4d    # iters: %3d\n",fps_cnt,imagePoints.size(),poseIters);
     printf("  Pose Estimate:%7s  %7s  %7s  %7s  %7s  %7s   [in/deg]\n","x","y","z","roll","pitch","yaw");
     printf("                %7.1f  %7.1f  %7.1f  %7.1f  %7.1f  %7.1f",
     reportState[0], reportState[1], reportState[2],

LED_Detector.cpp

@@ -30,7 +30,8 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
     vector<vector<Point> > contours;
     findContours(binaryImage,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
 
-
+    /// Jump back to here if we can't use previous state
+    not_detected_stop:
 
 
     /// We now have a vectorized contours object: vector<vector<Point> > contours
@@ -44,21 +45,18 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
     if (havePreviousState) // Need to initialize proximities
     {
         proximityVec.resize(NO_LEDS);  // This will resize 1st dimension to proximityVec[5][]
+        for (int i=0; i<NO_LEDS; i++)
+            proximityVec[i].resize(0);  // this will clear the contents of each subvector
 
-        // vector to keep closest points in
-//        for (int i=0; i<NO_LEDS; i++)
-//        {
-//            proximityVec[i].center = Point2f(NAN,NAN);
-//            proximityVec[i].radius = INFINITY;   // Record that we dont' see any nearby points
-//        }
-        float span_sq = pow((reprojImgPts[NO_LEDS-1].x - reprojImgPts[0].x),2) //compute the span to normalize by later
+        // Compute the wingspan based on reprojected image points from last frame
+        float span_sq = pow((reprojImgPts[NO_LEDS-1].x - reprojImgPts[0].x),2)
             + pow((reprojImgPts[NO_LEDS-1].y - reprojImgPts[0].y),2);
         span = sqrt(span_sq);
 
         // Determine the bounding rectangle of the lead UAV and scale up a little bit
         RotatedRect leaderROI = minAreaRect(reprojImgPts);
-        leaderROI.size.height += 75;
-        leaderROI.size.width  += 75;
+        leaderROI.size.height += 90;
+        leaderROI.size.width  += 90;
         Point2f verticies[4];
 
         // vectorize the bounding ROI
@@ -91,8 +89,8 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
             // if point does not lie within or lie on leaderROI RotatedRect, continue
             if (pointPolygonTest(leaderROI_contour,contourStruct.center,false) < 0 )
                 continue;
-            /// Uncomment to show blobs that appear inside ROI
-            //cv::circle(frame,contourStruct.center, 6, cv::Scalar(0,0,0), 2);
+            /// Uncomment to show/highlight blobs that appear inside ROI
+            cv::circle(frame,contourStruct.center, 6, cv::Scalar(255,255,255), 2);
         }
 
         if (params.filterByArea)
@@ -172,13 +170,13 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
                 contourStruct.color_delta += 360;
             contourStruct.color_delta = abs(contourStruct.color_delta);
 
-
             if (params.filterByColor)
             {
                 if ( contourStruct.color_delta > params.maxColor )
                     continue;
             }
 
+
             if (havePreviousState)
             {
                 // check each blob if in local raidus of previous image point
@@ -191,35 +189,39 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
                     // check if within local radius
                     if (radius < (params.localRadius * span / 100))
                     {
-//                        n_detected++;
-//                        if (radius < (proximityVec[i].radius)) // replace if better than current value
-//                        {
-//                            proximityVec[i].center = contourStruct.center;
-//                            proximityVec[i].radius = radius;
-//                        }
-
-                        n_detected++;
                         proximity tmpProx;
                             tmpProx.center = contourStruct.center;
                             tmpProx.radius = radius;
                         proximityVec[i].push_back(tmpProx);
                     }
                 }
+
             }
 
         }
 
         contourStructVec.push_back(contourStruct);
     }
 
-    // If we don't detect at least 2 of the LEDs in current frame, don't use previous information
-    // (prevents us from holding onto a past state that may not be true)
-    //if (n_detected < 3)
-    //    havePreviousState = false;
 
+    if (!havePreviousState && params.sortByColor)
+    {
+        sort(contourStructVec.begin(), contourStructVec.end(), LED_Detector::sortByColor);
+
+        // populate the vector of image points from the contourStructVec
+        int stopHere = (contourStructVec.size() > params.maxBlobs) ? params.maxBlobs : contourStructVec.size();
+        for (int i=0; i<stopHere; i++)
+        {
+            leds.push_back(contourStructVec[i].center);
+        }
+    }
+
+    /// This is the beginning of modified code to selectively select "best" image points
+    /// and handle duplicates
     if (havePreviousState)
     {
-        // Flatten vector to more easily access elements
+        // Flatten vector to more easily access elements for duplicate detection
+        // (don't have to traverse across two dimensions)
         vector<proximity*> flat; // "flattened" vector (points to elements of proximityVec)
         for (int i=0; i<NO_LEDS; i++)
         {
@@ -257,7 +259,8 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
             sort(groups[i].begin(),groups[i].end(),compByRadius);
 
 
-        // Eliminate any duplicate points (keeping the one(s) with the lowest radius)
+        // Eliminate any duplicate points (choosing to keep the one(s) with the lowest radius)
+        n_detected = NO_LEDS;
         for (int i=0; i<groups.size(); i++)
         {
             for (int j=1; j<groups[i].size(); j++) // erase everything after the first element
@@ -270,24 +273,25 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
                 if (proximityVec[i][j].delete_me)
                     proximityVec[i].erase(proximityVec[i].begin() + j);
             }
+
+            // If any rows are empty, subtract from the detected LED count
+            if (proximityVec[i].empty())
+                n_detected--;
+        }
+
+        // Assume we don't see the aircraft if we see fewer than 3 LEDs
+        // and use normal detection logic
+        if (n_detected < 3)
+        {
+            havePreviousState = false;
+            goto not_detected_stop;     // go back to beginning with assumption that we don't have previous state
         }
 
 
         // Sort by radius
         for (int i=0; i<NO_LEDS; i++)
             sort(proximityVec[i].begin(),proximityVec[i].end());  // sort by increasing radius
 
-        cout << endl << "SORTED BY RADIUS (DUPLICATES REMOVED)" << endl;
-        for (int i=0; i<NO_LEDS; i++)
-        {
-            cout << " Group " << i << endl;
-            for (int j=0; j<proximityVec[i].size(); j++)
-            {
-                cout << "  Prox " << j << endl;
-                cout << "   center: " << proximityVec[i][j].center.x << " " << proximityVec[i][j].center.y << endl;
-                cout << "   radius: " << proximityVec[i][j].radius << endl;
-            }
-        }
 
         // Replace any "missing" points with reprojImgPts from last time
         for (int i=0; i<NO_LEDS; i++)
@@ -301,18 +305,19 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
         }
 
 
-        // Return the first 5 points in order, then add points in successive layers (selected randomly) until we reach maximum (N) points
+        // Return the first 5 points in order, then add points in successive layers
+        // (selected randomly) until we reach maximum (N) points
         const int LEDs_max = 8;
 
-        vector<Point2f> LEDs;
         // Pass the first LEDs
         for (int i=0; i<NO_LEDS; i++)
-            LEDs.push_back(proximityVec[i][0].center);
+            leds.push_back(proximityVec[i][0].center);
+
 
         // Go along successive layers to add "extra" points choosing which to pass by random
         vector<Point2f*> layer;
         int layer_no = 0;
-        while (LEDs.size() < LEDs_max)
+        while (leds.size() < LEDs_max)
         {
 
             // Populate the next layer if we have extracted all elements from previous one
@@ -329,49 +334,11 @@ bool LED_Detector::findLEDs(const cv::Mat& RGBImage, cv::Mat &grayImage, cv::Mat
                 break;
 
             int randomPick = rand() % layer.size();
-            LEDs.push_back(*layer[randomPick]);
+            leds.push_back(*layer[randomPick]);
             layer.erase(layer.begin() + randomPick);
         }
 
     }
-    else if (params.sortByColor)
-    {
-        sort(contourStructVec.begin(), contourStructVec.end(), LED_Detector::sortByColor);
-
-        // populate the vector of image points from the contourStructVec
-        int stopHere = (contourStructVec.size() > params.maxBlobs) ? params.maxBlobs : contourStructVec.size();
-        for (int i=0; i<stopHere; i++)
-        {
-            leds.push_back(contourStructVec[i].center);
-        }
-    }
-
-
-    #ifdef LED_DETECTOR_DEBUG
-    Mat debugImage = RGBImage.clone();
-    for (int i=0; i<contours.size(); i++)
-    {
-        drawContours(debugImage,contours,i,Scalar(153,255,255),-1);
-        drawContours(debugImage,contours,i,Scalar(0,0,0),1);
-    }
-
-    for (int i=0; i<leds.size(); i++)
-    {
-        cout << leds[i] << endl;
-        circle(debugImage,leds[i],3,Scalar(255,0,0),2);
-        circle(debugImage,leds[i],6,Scalar(0,0,255),2);
-        char msg[50];
-        sprintf(msg,"%d",i);
-        putText(debugImage,msg,leds[i],FONT_HERSHEY_PLAIN,2,Scalar(255,255,255),2);
-    }
-
-
-    imshow("LED_Detector_Debug",debugImage);
-    waitKey(1);
-
-    imshow("grayImage",grayImage);
-    waitKey(0);
-    #endif /* LED_DETECTOR_DEBUG */
 
     if (havePreviousState)
         return true;

PnPObj.cpp

@@ -359,7 +359,8 @@ int PnPObj::localizeUAV(const std::vector<cv::Point2f> &imagePoints_IN, std::vec
 
 				PnPObj::solve();	 // WARNING:  Modifies private member function values of PnPObj!
 
-				if (scaledReprojErr < perrTol){
+				// Relax the tolerance if we have a previous frame to help us out
+				if (scaledReprojErr < perrTol || (scaledReprojErr < serrTol && skipCorrelation)){
 					poseErr = scaledReprojErr;
 					poseState = PnPObj::getState();
 
@@ -378,8 +379,8 @@ int PnPObj::localizeUAV(const std::vector<cv::Point2f> &imagePoints_IN, std::vec
 					error_buff.push_back(scaledReprojErr);
 					PnPObj_buff.push_back(*this); // append this instance of PnPObj to buffer
 				}
-                if (skipCorrelation)  //only go through one iteration if we are providing image pts in order
-                    break;
+//                if (skipCorrelation)  //only go through one iteration if we are providing image pts in order
+//                    break;
 			}
 		}