Merge pull request #330 from lasp/feature/1135-brightness-increase-threshold

Feature/1135 brightness increase threshold

Author: patkenneally

src/fswAlgorithms/imageProcessing/centerOfBrightness/_UnitTest/test_centerOfBrightness.py

@@ -94,6 +94,8 @@ def centerOfBrightnessTest(show_plots, image, blur, saveTest, validImage, saveIm
     if windowCenter.all() != 0 and windowWidth != 0 and windowHeight != 0:
         moduleConfig.setWindowCenter(windowCenter)
         moduleConfig.setWindowSize(windowWidth, windowHeight)
+    brightnessIncreaseThreshold = 0.0
+    moduleConfig.setRelativeBrightnessIncreaseThreshold(brightnessIncreaseThreshold)
     unitTestSim.AddModelToTask(unitTaskName, moduleConfig)
 
     numberOfPointsBrightnessAverage = 3
@@ -131,7 +133,7 @@ def centerOfBrightnessTest(show_plots, image, blur, saveTest, validImage, saveIm
 
     # run simulation for 5 time steps (excluding initial time step at 0 ns), scale brightness each time step
     # necessary to test rolling brightness average
-    scaler = np.array([1.0, 0.9, 0.8, 0.7, 0.6])
+    scaler = np.array([0.5, 0.6, 0.8, 0.3, 0.9])
     brightness_ref = np.zeros([len(scaler)])
     brightnessAverage_ref = np.zeros([len(scaler)])
     for i in range(0, len(scaler)):
@@ -150,8 +152,8 @@ def centerOfBrightnessTest(show_plots, image, blur, saveTest, validImage, saveIm
             lower_idx = max(0, i-(numberOfPointsBrightnessAverage-1))
             brightnessAverage_ref[i] = np.mean(brightness_ref[lower_idx:i+1])
 
-    center = dataLog.centerOfBrightness[-1,:]
-    pixelNum = dataLog.pixelsFound[-1]
+    center = dataLog.centerOfBrightness[0, :]
+    pixelNum = dataLog.pixelsFound[0]
     brightnessAverage = dataLog.rollingAverageBrightness
 
     output_image = Image.new("RGB", input_image.size)
@@ -203,7 +205,7 @@ def centerOfBrightnessTest(show_plots, image, blur, saveTest, validImage, saveIm
 
         np.testing.assert_allclose(brightnessAverage,
                                    brightnessAverage_ref,
-                                   rtol=tolerance,
+                                   rtol=0.001,
                                    atol=0,
                                    err_msg='Variable: brightnessAverage',
                                    verbose=True)

src/fswAlgorithms/imageProcessing/centerOfBrightness/centerOfBrightness.cpp

@@ -92,15 +92,29 @@ void CenterOfBrightness::UpdateState(uint64_t CurrentSimNanos)
     if (!locations.empty()){
         std::pair<Eigen::Vector2d, double> cobData;
         cobData = this->computeWeightedCenterOfBrightness(locations);
+
+        double averageBrightnessOld = 0.0;
+        if (this->brightnessHistory.rows() > 0){
+            averageBrightnessOld = this->brightnessHistory.mean();
+        }
         this->updateBrightnessHistory(cobData.second);
+        double averageBrightnessNew = this->brightnessHistory.mean();
+        double brightnessIncrease = 0.0;
+        if (averageBrightnessOld > 0.0){
+            brightnessIncrease = (averageBrightnessNew - averageBrightnessOld)
+                    / averageBrightnessOld;
+        }
 
-        cobBuffer.valid = true;
-        cobBuffer.timeTag = this->sensorTimeTag;
-        cobBuffer.cameraID = imageBuffer.cameraID;
-        cobBuffer.centerOfBrightness[0] = cobData.first[0];
-        cobBuffer.centerOfBrightness[1] = cobData.first[1];
-        cobBuffer.pixelsFound = static_cast<int32_t> (locations.size());
-        cobBuffer.rollingAverageBrightness = this->brightnessHistory.mean();
+        /*! If brightness increase is less than brightness increase threshold, do not validate image */
+        if (brightnessIncrease >= this->relativeBrightnessIncreaseThreshold){
+            cobBuffer.valid = true;
+            cobBuffer.timeTag = this->sensorTimeTag;
+            cobBuffer.cameraID = imageBuffer.cameraID;
+            cobBuffer.centerOfBrightness[0] = cobData.first[0];
+            cobBuffer.centerOfBrightness[1] = cobData.first[1];
+            cobBuffer.pixelsFound = static_cast<int32_t> (locations.size());
+        }
+        cobBuffer.rollingAverageBrightness = averageBrightnessNew;
     }
 
     this->opnavCOBOutMsg.write(&cobBuffer, this->moduleID, CurrentSimNanos);
@@ -273,3 +287,20 @@ Eigen::VectorXi CenterOfBrightness::getWindowSize() const
     Eigen::VectorXi center = {this->windowWidth, this->windowHeight};
     return center;
 }
+
+/*! Set threshold for the increase in brightness for images not to be invalidated
+    @param double increaseThreshold
+    @return void
+    */
+void CenterOfBrightness::setRelativeBrightnessIncreaseThreshold(double increaseThreshold)
+{
+    this->relativeBrightnessIncreaseThreshold = increaseThreshold;
+}
+
+/*! Get threshold for the increase in brightness for images not to be invalidated
+    @return double increaseThreshold
+    */
+double CenterOfBrightness::getRelativeBrightnessIncreaseThreshold() const
+{
+    return this->relativeBrightnessIncreaseThreshold;
+}

src/fswAlgorithms/imageProcessing/centerOfBrightness/centerOfBrightness.h

@@ -45,6 +45,8 @@ class CenterOfBrightness: public SysModel {
     Eigen::VectorXi getWindowCenter() const;
     void setWindowSize(int32_t width, int32_t height);
     Eigen::VectorXi getWindowSize() const;
+    void setRelativeBrightnessIncreaseThreshold(double increaseThreshold);
+    double getRelativeBrightnessIncreaseThreshold() const;
 
 private:
     std::vector<cv::Vec2i> extractBrightPixels(cv::Mat image);
@@ -75,6 +77,7 @@ class CenterOfBrightness: public SysModel {
     Eigen::Vector2i windowPointBottomRight{};  //!< [px] bottom right point of window
     bool validWindow = false;            //!< [px] true if window is set, false if center, height, or width equal 0
     Eigen::VectorXd brightnessHistory{};    //!< [-] brightness history to be used for rolling average
+    double relativeBrightnessIncreaseThreshold{};  //!< [-] minimum relative brightness increase (if less, invalidated)
     /* OpenCV specific arguments needed for finding all non-zero pixels*/
     cv::Mat imageGray;                   //!< [cv mat] Gray scale image for weighting
 };

src/fswAlgorithms/imageProcessing/centerOfBrightness/centerOfBrightness.rst

@@ -63,8 +63,10 @@ The normalized total brightness is equal to
     I_\mathrm{tot, normalized} = \frac{I_\mathrm{tot}}{255}
 
 and the rolling average is computed over the last :math:`N` time steps, as specified by numberOfPointsBrightnessAverage.
+If the relative increase of the rolling brightness average from one time step to the next is below the threshold
+brightnessIncreaseThreshold, the image is tagged as invalid.
 
-If the incomping image is not valid, or there were no pixels above the threshold, the image is tagged as invalid.
+If the incoming image is not valid, or there were no pixels above the threshold, the image is tagged as invalid.
 Downstream algorithms can therefore know when to skip a measurement.
 
 User Guide
@@ -93,6 +95,10 @@ This section is to outline the steps needed to setup a Center of Brightness in P
 
     cobAlgorithm.numberOfPointsBrightnessAverage = 5
 
+#. Specify the minimum relative brightness increase of the rolling average of total brightness (optional)::
+
+    moduleConfig.setRelativeBrightnessIncreaseThreshold(0.1)
+
 #. Subscribe to the image message output by the camera model or visualization interface::
 
     cobAlgorithm.imageInMsg.subscribeTo(imgInMsg)