Fix camera recognition (#5967)

* Fix scale of bounding sphere for some geometries

* Avoid adding extra padding to detection using bounding sphere

* Use scale from matrix to avoid assertion failure in clampedAcos

* Fix frustum representation if scaled

* Avoid unnecessary data copying

* Fix frustum representation when scale is changed

* Revert fixes to scaled camera frustum visualization

* Fix computation of Camera vertical FOV

* Fix projection for spherical/cylindrical and lens effects

* Apply lens distortion to segmented image

* Fix projectOnImage for spherical projection and lens distortion

* Fix some issues with projection on image and object in frustum check

* Fix loop

* Fix indentation

* Fix detection on the frustum border

* Fix edge case

* Fix distance to object point

* Improve test viewpoint position and adjust position on image value

* Fix indent

* Add entry in changelog

* Improve fix

* Remove not needed computations

* Test recognition for spherical and cylindrical cameras

* Fix detected object size for spherical and cylindrical cameras

* Fix detected objects info not displayed from overlay without occlusion

* update ChangeLog

* Fix test

* Fix variable initialization

* Fix ClangFormat error

* Cleanup WbAffinePlane and remove multiple normalization

* Fix recognized size of big objects partly behind the device

* Do not update devices if the robot is being deleted

* Fix GL_INVALID_VALUE

* Update segmentation texture GLId after recreating the camera data

* Fix test

* Adjust camera recognition test result

* Clamp UV coordinates

---------

Co-authored-by: Olivier Michel <[email protected]>

Author: stefaniapedrazzi

docs/reference/changelog-r2023.md

@@ -52,6 +52,10 @@ Released on ??
     - Fixed bug causing Webots to hang when requesting multiple poses while pose-tracking multiple nodes ([#5952](https://github.com/cyberbotics/webots/pull/5952)).
     - Fixed physics state updates of [Solid](solid.md) nodes added to a descendant [`Joint.endPoint`](joint.md) during simulation ([#5961](https://github.com/cyberbotics/webots/pull/5961)).
     - Fixed unwanted altitude change when reaching a target waypoint in `mavic2pro_patrol.c` ([#5981](https://github.com/cyberbotics/webots/pull/5981)).
+    - Fixed the extern controller connection to a target Webots instance when a snap one is running. ([#6002](https://github.com/cyberbotics/webots/pull/6002)).
+    - Fixed various issues related to the camera's [Recognition](recognition.md) functionality including image annotations in case of spherical and cylindrical projections ([#5967](https://github.com/cyberbotics/webots/pull/5967)).
+    - Fixed [Camera](camera.md) segmentation image if a [Lens](lens.md) is defined. ([#5967](https://github.com/cyberbotics/webots/pull/5967)).
+    - Fixed detected object information not available when moving the mouse over the [Camera](camera.md) overlay after the controller termination if [Recognition.occlusion](recognition.md) is set to FALSE. ([#5967](https://github.com/cyberbotics/webots/pull/5967)).
     - Fixed the extern controller connection to a target Webots instance when a snap one is running ([#6002](https://github.com/cyberbotics/webots/pull/6002)).
     - Fixed the double downloading of meshes ([#6034](https://github.com/cyberbotics/webots/pull/6034)).
     - Fixed generation of moment of inertia matrix based on bounding box ([#6048](https://github.com/cyberbotics/webots/pull/6048)).

src/webots/maths/WbAffinePlane.cpp

@@ -23,4 +23,5 @@ void WbAffinePlane::from3Points(const WbVector3 &P, const WbVector3 &Q, const Wb
   mC = u[0] * v[1] - u[1] * v[0];
   // Compute the scalar product of u cross v with OP
   mD = mA * P.x() + mB * P.y() + mC * P.z();
+  normalize();
 }

src/webots/maths/WbAffinePlane.hpp

@@ -16,7 +16,8 @@
 #define WB_AFFINE_PLANE_HPP
 
 //
-// Description: data structure for a 3D affine plane defined by the equation ax + by + cz = d
+// Description: data structure for a 3D affine plane defined by the equation ax + by + cz = d.
+//              Normal vector (a, b, c) is a unit vector.
 //
 
 #include <cmath>
@@ -29,13 +30,12 @@ class WbAffinePlane {
 
   // construct from coefficients
   WbAffinePlane(const WbAffinePlane &other) : mA(other.mA), mB(other.mB), mC(other.mC), mD(other.mD) {}
-  WbAffinePlane(double a, double b, double c, double d) : mA(a), mB(b), mC(c), mD(d) {}
-  explicit WbAffinePlane(const double p[4]) : mA(p[0]), mB(p[1]), mC(p[2]), mD(p[3]) {}
   // construct from a normal vector and a scalar
-  WbAffinePlane(const WbVector3 &v, double d) : mA(v.x()), mB(v.y()), mC(v.z()), mD(d) {}
+  WbAffinePlane(const WbVector3 &v, double d) : mA(v.x()), mB(v.y()), mC(v.z()), mD(d) { normalize(); }
   // construct from a normal vector and a point
   WbAffinePlane(const WbVector3 &v, const WbVector3 &P) {
     mA = v.x(), mB = v.y(), mC = v.z(), mD = v.x() * P.x() + v.y() * P.y() + v.z() * P.z();
+    normalize();
   }
   // construct from 3 points
   WbAffinePlane(const WbVector3 &P, const WbVector3 &Q, const WbVector3 &R) { from3Points(P, Q, R); }
@@ -49,68 +49,9 @@ class WbAffinePlane {
   double d() const { return mD; }
   WbVector3 normal() const { return WbVector3(mA, mB, mC); }
 
-  // setters
-  void setA(double a) { mA = a; }
-  void setB(double b) { mB = b; }
-  void setC(double c) { mC = c; }
-  void setD(double d) { mD = d; }
-
-  //
-  void redefine(double a, double b, double c, double d) {
-    mA = a;
-    mB = b;
-    mC = c;
-    mD = d;
-  }
-  void redefine(const double p[4]) {
-    mA = p[0];
-    mB = p[1];
-    mC = p[2];
-    mD = p[3];
-  }
-  void redefine(const WbVector3 &P, const WbVector3 &Q, const WbVector3 &R) { from3Points(P, Q, R); }
   void redefine(const WbVector3 &v, const WbVector3 &P) {
     mA = v.x(), mB = v.y(), mC = v.z(), mD = v.x() * P.x() + v.y() * P.y() + v.z() * P.z();
-  }
-  // normalize
-  void normalize();
-
-  // 3 special planes
-  void setXZ() {
-    mA = 0.0;
-    mB = 1.0;
-    mC = 0.0;
-    mD = 0.0;
-  }
-  void setXZ(double d) {
-    mA = 0.0;
-    mB = 1.0;
-    mC = 0.0;
-    mD = d;
-  }
-  void setXY() {
-    mA = 0.0;
-    mB = 0.0;
-    mC = 1.0;
-    mD = 0.0;
-  }
-  void setXY(double d) {
-    mA = 0.0;
-    mB = 0.0;
-    mC = 1.0;
-    mD = d;
-  }
-  void setYZ() {
-    mA = 1.0;
-    mB = 0.0;
-    mC = 0.0;
-    mD = 0.0;
-  }
-  void setYZ(double d) {
-    mA = 1.0;
-    mB = 0.0;
-    mC = 0.0;
-    mD = d;
+    normalize();
   }
 
   // assignment: P1 = P2
@@ -123,17 +64,16 @@ class WbAffinePlane {
   }
 
   // pseudo-distance to a 3D point
-  // this is the signed distance from P to the plane if (a, b, c) is a unit vector, the sign is positive if P lies in the upper
-  // plane determined by (a, b, c) and negative otherwise.
+  // this is the signed distance from P to the plane,
+  // the sign is positive if P lies in the upper plane determined by (a, b, c) and negative otherwise.
   double distance(const WbVector3 &v) const { return mA * v.x() + mB * v.y() + mC * v.z() - mD; }
-  double distance(const double v[3]) const { return mA * v[0] + mB * v[1] + mC * v[2] - mD; }
-  double distance(double x, double y, double z) const { return mA * x + mB * y + mC * z - mD; }
 
   // project a vector on this plane, returns the project vector
-  WbVector3 vectorProjection(const WbVector3 &v) const { return v - normal() * (v.dot(normal()) / normal().length2()); }
+  WbVector3 vectorProjection(const WbVector3 &v) const { return v + normal() * v.dot(normal()); }
 
 private:
   double mA, mB, mC, mD;
+  void normalize();
 };
 
 inline void WbAffinePlane::normalize() {

src/webots/nodes/WbAbstractCamera.cpp

@@ -508,6 +508,9 @@ void WbAbstractCamera::createWrenCamera() {
   bool enableAntiAliasing = antiAliasing() && !WbPreferences::instance()->value("OpenGL/disableAntiAliasing", true).toBool();
   mWrenCamera = new WbWrenCamera(wrenNode(), width(), height(), nearValue(), minRange(), maxRange(), fieldOfView(), mCharType,
                                  enableAntiAliasing, mProjection->value());
+}
+
+void WbAbstractCamera::applyCameraSettings() {
   updateBackground();
 
   connect(mWrenCamera, &WbWrenCamera::cameraInitialized, this, &WbAbstractCamera::applyCameraSettingsToWren);
@@ -795,7 +798,6 @@ void WbAbstractCamera::applyFrustumToWren() {
   const float w = width();
   const float h = height();
   const float fovX = mFieldOfView->value();
-  const float fovY = WbWrenCamera::computeFieldOfViewY(fovX, w / h);  // fovX -> fovY
   const float t = tanf(fovX / 2.0f);
   const float dw1 = n * t;
   const float dh1 = dw1 * h / w;
@@ -853,12 +855,13 @@ void WbAbstractCamera::applyFrustumToWren() {
   const float zero[3] = {0.0f, 0.0f, 0.0f};
   // Creation of the external outline of the frustum (4 lines)
   if (!isPlanarProjection()) {
+    const float fovY = WbWrenCamera::computeFieldOfViewY(fovX, w / h);  // fovX -> fovY
     const float angleY[4] = {-fovY / 2.0f, -fovY / 2.0f, fovY / 2.0f, fovY / 2.0f};
     const float angleX[4] = {fovX / 2.0f, -fovX / 2.0f, -fovX / 2.0f, fovX / 2.0f};
     for (int k = 0; k < 4; ++k) {
       const float helper = cosf(angleY[k]);
       // get x, y and z from the spherical coordinates
-      float y = 0.0f;
+      float y;
       if (angleY[k] > M_PI_4 || angleY[k] < -M_PI_4)
         y = f * cosf(angleY[k] + M_PI_2) * sinf(angleX[k]);
       else

src/webots/nodes/WbAbstractCamera.hpp

@@ -120,6 +120,8 @@ class WbAbstractCamera : public WbRenderingDevice {
 
   virtual int size() const = 0;
 
+  void applyCameraSettings();
+
   // Wren methods
   virtual void createWrenCamera();
   void createWrenOverlay() override;
@@ -183,7 +185,7 @@ protected slots:
   void updateMotionBlur();
   void updateNoise();
   void updateLens();
-  void applyLensToWren();
+  virtual void applyLensToWren();
   void removeInvisibleNodeFromList(QObject *node);
 
   virtual void updateFrustumDisplayIfNeeded(int optionalRendering) {}