From 9d916b7f9964bbbfff3a0e35f496b836b5e40a53 Mon Sep 17 00:00:00 2001
From: fabien servant <fabien.servant@interdigital.com>
Date: Wed, 12 Aug 2020 17:26:51 +0200
Subject: [PATCH 1/2] bug in minimization

---
 src/software/pipeline/main_panoramaInit.cpp | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/software/pipeline/main_panoramaInit.cpp b/src/software/pipeline/main_panoramaInit.cpp
index 2e05df646b..cf3a2a45e0 100644
--- a/src/software/pipeline/main_panoramaInit.cpp
+++ b/src/software/pipeline/main_panoramaInit.cpp
@@ -331,7 +331,11 @@ class CircleDetector {
         double cx = pt(0) - best_params(0);
         double cy = pt(1) - best_params(1);
         double r = best_params(2);
-        double dist = sqrt(cx * cx + cy * cy) - r;
+        double normsq = cx * cx + cy * cy;
+        double norm = sqrt(normsq);
+        double dist = norm - r;
+
+        
 
         double w = 0.0;
         if (dist < c) {
@@ -341,13 +345,13 @@ class CircleDetector {
         }
 
         Eigen::Vector3d J;
-        if (std::abs(dist) < 1e-12) {
+        if (std::abs(normsq) < 1e-12) {
           J.fill(0);
           J(2) = -w;
         }
         else {
-          J(0) = - w * cx / dist;
-          J(1) = - w * cy / dist;
+          J(0) = - w * cx / norm;
+          J(1) = - w * cy / norm;
           J(2) = - w;
         }
 

From 4e02c97f202da508ea5ef45aa9edd87a5ef492ad Mon Sep 17 00:00:00 2001
From: fabien servant <fabien.servant@interdigital.com>
Date: Wed, 12 Aug 2020 17:40:21 +0200
Subject: [PATCH 2/2] add masks to polar image

---
 src/software/pipeline/main_panoramaInit.cpp | 34 ++++++++++++++-------
 1 file changed, 23 insertions(+), 11 deletions(-)

diff --git a/src/software/pipeline/main_panoramaInit.cpp b/src/software/pipeline/main_panoramaInit.cpp
index cf3a2a45e0..c1bd3197d7 100644
--- a/src/software/pipeline/main_panoramaInit.cpp
+++ b/src/software/pipeline/main_panoramaInit.cpp
@@ -142,12 +142,17 @@ class CircleDetector {
 
     double max_rx = std::max(double(source.Width()) - level_centerx, level_centerx);
     double max_ry = std::max(double(source.Height()) - level_centery, level_centery);
+
     double max_radius = std::min(max_rx, max_ry);
+    //Just in case the smallest side is cropped inside the circle.
+    max_radius *= 1.5;
+
     size_t max_radius_i = size_t(std::ceil(max_radius));
     double angles_bins = size_t(std::ceil(max_radius * M_PI)); 
 
-    image::Image<float> polarImage(max_radius_i, angles_bins);
-    if (!buildPolarImage(polarImage, source, level_centerx, level_centery)) {
+    image::Image<float> polarImage(max_radius_i, angles_bins, true, 0.0f);
+    image::Image<unsigned char> polarImageMask(max_radius_i, angles_bins, true, 0);
+    if (!buildPolarImage(polarImage, polarImageMask, source, level_centerx, level_centery)) {
       return false;
     }
 
@@ -158,7 +163,7 @@ class CircleDetector {
     int min_radius = 8;
     int radius = min_radius;// * pow(2, diff);
     
-    if (!buildGradientImage(gradientImage, polarImage, radius)) {
+    if (!buildGradientImage(gradientImage, polarImage, polarImageMask, radius)) {
       return false;
     }
 
@@ -449,12 +454,11 @@ class CircleDetector {
     return true;
   }
 
-  bool buildPolarImage(image::Image<float> & dst, const image::Image<float> & src, float center_x, float center_y) {
+  bool buildPolarImage(image::Image<float> & dst, image::Image<unsigned char> & dstmask, const image::Image<float> & src, float center_x, float center_y) {
 
     image::Sampler2d<image::SamplerLinear> sampler;
     size_t count_angles = dst.Height();
 
-    
 
     for (int angle = 0; angle < count_angles; angle++) {
       double rangle = angle * (2.0 * M_PI / double(count_angles));
@@ -467,16 +471,19 @@ class CircleDetector {
         double y = center_y + sangle * double(amplitude);
 
         dst(angle, amplitude) = 0;
+        dstmask(angle, amplitude) = 0;
+
         if (x < 0 || y < 0) continue;
         if (x >= src.Width() || y >= src.Height()) continue;
         dst(angle, amplitude) = sampler(src, y, x);
+        dstmask(angle, amplitude) = 255;
       }
     }
     
     return true;
   }
 
-  bool buildGradientImage(image::Image<float> & dst, const image::Image<float> & src, size_t radius_size) {
+  bool buildGradientImage(image::Image<float> & dst, const image::Image<float> & src, const image::Image<unsigned char> & srcMask, size_t radius_size) {
 
     /*Build gradient for x coordinates image */
     dst.fill(0);
@@ -493,24 +500,29 @@ class CircleDetector {
         float sum_inside = 0.0;
         float sum_outside = 0.0;
 
+        unsigned char valid = 255;
+
         for (int dx = -kernel_radius; dx < 0; dx++) {
           sum_inside += src(angle, amplitude + dx);
+          valid &= srcMask(angle, amplitude + dx);
         }
         for (int dx = 1; dx <= kernel_radius * 2; dx++) {
           sum_outside += src(angle, amplitude + dx);
+          valid &= srcMask(angle, amplitude + dx);
         }
 
-        dst(angle, amplitude) = sum_inside - sum_outside;
+        if (valid) {
+          dst(angle, amplitude) = std::max(0.0f, (sum_inside - sum_outside));
+        }
+        else {
+          dst(angle, amplitude) = 0.0f;
+        }
       }
     }
     
     return true;
   }
 
-  /*void drawCircle(image::Image<image::RGBfColor> & dest) {
-    image::DrawCircle(_center_x, _center_y, _radius, image::RGBfColor(1.0f), &dest);
-  }*/
-
   double getCircleCenterX() const {
     return _center_x;
   }