Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
vpRBProbabilistic3DDriftDetector.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  */
30 
31 #include <visp3/rbt/vpRBProbabilistic3DDriftDetector.h>
32 
33 #include <visp3/core/vpRect.h>
34 #include <visp3/core/vpPixelMeterConversion.h>
35 #include <visp3/core/vpDisplay.h>
36 
37 #include <visp3/rbt/vpRBFeatureTracker.h>
38 
39 #if defined(VISP_HAVE_NLOHMANN_JSON)
40 #include VISP_NLOHMANN_JSON(json.hpp)
41 #endif
42 
43 BEGIN_VISP_NAMESPACE
44 
46  const vpRBFeatureTrackerInput &frame,
47  const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &/*cprevTo*/)
48 {
49  const vpHomogeneousMatrix &cprevTo = frame.renders.cMo;
50  const vpTranslationVector t = cprevTo.inverse().getTranslationVector();
51 
52  if (m_points.size() > 0) {
53  // Step 0: project all points
54 #ifdef VISP_HAVE_OPENMP
55 #pragma omp parallel for
56 #endif
57  for (vpStored3DSurfaceColorPoint &p : m_points) {
58  p.update(cTo, cprevTo, frame.cam);
59  }
60 
61  // Step 1: gather points visible in both images and in render
62  std::vector<vpStored3DSurfaceColorPoint *> visiblePoints;
63  for (vpStored3DSurfaceColorPoint &p : m_points) {
64  p.visible = true;
65  if (
66  p.projPrevPx[0] < 2 || static_cast<unsigned int>(p.projPrevPx[0]) >= frame.IRGB.getWidth() - 2
67  || p.projPrevPx[1] < 2 || static_cast<unsigned int>(p.projPrevPx[1]) >= frame.IRGB.getHeight() - 2
68  || p.projCurrPx[0] < 2 || static_cast<unsigned int>(p.projCurrPx[0]) >= frame.IRGB.getWidth() - 2
69  || p.projCurrPx[1] < 2 || static_cast<unsigned int>(p.projCurrPx[1]) >= frame.IRGB.getHeight() - 2) {
70  p.visible = false; // Point is outside of either current or previous image, ignore it
71  continue;
72  }
73 
74  // if (fabs(p.currX[2] - ZcurrMap) > m_maxError3D || fabs(p.prevX[2] - ZprevMap) > m_maxError3D) {
75  // continue; // Depth is wrong: probable occlusion, ignore it
76  // }
77 
78  float ZrenderMap = frame.renders.depth[p.projPrevPx[1]][p.projPrevPx[0]];
79  // Version 2: compare previous projection with render, this does not filter occlusions
80  if (ZrenderMap == 0.f || fabs(p.prevX[2] - ZrenderMap) > m_maxError3D) {
81  p.visible = false;
82  continue;
83  }
84 
85  vpRGBf normalObject = frame.renders.normals[p.projPrevPx[1]][p.projPrevPx[0]];
86 
87  vpColVector cameraRay({ t[0] - p.X[0], t[1] - p.X[1], t[2] - p.X[2] });
88 
89  cameraRay.normalize();
90  double angle = acos(vpColVector::dotProd(vpColVector({ normalObject.R, normalObject.G, normalObject.B }).normalize(), cameraRay));
91  if (angle > vpMath::rad(75)) {
92  p.visible = false;
93  continue;
94  }
95 
96  // Filter points that are too close to the silhouette edges
97  if (frame.silhouettePoints.size() > 0) {
98  for (const vpRBSilhouettePoint &sp: frame.silhouettePoints) {
99  if (std::pow(static_cast<double>(sp.i) - p.projPrevPx[1], 2) + std::pow(static_cast<double>(sp.j) - p.projPrevPx[0], 2) < vpMath::sqr(3)) {
100  p.visible = false;
101  break;
102  }
103  }
104  }
105  // Version 3: could be using version 1 and 2. If 1 is wrong but 2 is ok, then there is an issue that is not self occlusion
106  // We could reweigh the error by the number of problematic points
107  // ...
108 
109  if (p.visible) {
110  visiblePoints.push_back(&p);
111  }
112  }
113 
114  if (visiblePoints.size() > 0) {
115  // // Compute sample weight
116  // double maxTrace = 0.0;
117 
118  // for (vpStored3DSurfaceColorPoint *p : visiblePoints) {
119  // double trace = p->stats.trace();
120  // if (trace > maxTrace) {
121  // maxTrace = trace;
122  // }
123  // }
124  // maxTrace = std::max(maxTrace, 80.0);
125  double weightSum = 0.0;
126  m_score = 0.0;
127  for (vpStored3DSurfaceColorPoint *p : visiblePoints) {
128  double maxProba = 0.0;
129  vpRGBf minColor;
130  const bool hasCorrectDepth = frame.hasDepth() && frame.depth[p->projPrevPx[1]][p->projPrevPx[0]] > 0.f;
131  const double Z = hasCorrectDepth ? frame.depth[p->projPrevPx[1]][p->projPrevPx[0]] : 0.0;
132  double depthError = Z > 0 ? fabs(p->prevX[2] - Z) : 0.0;
133  double probaDepth = 1.0;
134  if (hasCorrectDepth) {
135  probaDepth = 1.0 - erf((depthError) / (m_depthSigma * sqrt(2.0)));
136  }
137  // double weight = 1.0 - ((p->stats.trace() / maxTrace));
138  // if (weight < 0.0) {
139  // throw vpException(vpException::badValue, "Got invalid weight");
140  // }
141  double weight = 1.0;
142 
143  vpRGBf averageColor(0.f, 0.f, 0.f);
144  for (int i = -1; i < 2; ++i) {
145  for (int j = -1; j < 2; ++j) {
146  const vpRGBa currentColor = frame.IRGB[p->projCurrPx[1] + i][p->projCurrPx[0] + j];
147  averageColor.R += currentColor.R;
148  averageColor.G += currentColor.G;
149  averageColor.B += currentColor.B;
150  }
151  }
152  averageColor = averageColor * (1.0 / 9.0);
153  // const vpRGBf c(currentColor.R, currentColor.G, currentColor.B);
154  const vpRGBf c(averageColor);
155 
156  const double probaColor = p->stats.probability(c);
157  const double proba = probaColor * probaDepth;
158  if (probaDepth > 1.f || probaDepth < 0.0) {
159  throw vpException(vpException::badValue, "Wrong depth probability");
160  }
161  if (proba > maxProba) {
162  maxProba = proba;
163  minColor = c;
164  }
165 
166  m_score += maxProba * weight;
167  weightSum += weight;
168  p->updateColor(minColor, m_colorUpdateRate * probaDepth);
169  }
170  m_score /= (weightSum);
171  }
172  else {
173  m_score = 1.0;
174  }
175  }
176  else {
177  m_score = 1.0;
178  }
179 
180  // Step 4: Sample bb to add new visible points
181  const vpHomogeneousMatrix oMcprev = cprevTo.inverse();
182  vpColVector cX(4, 1.0);
183  vpColVector oX(4, 1.0);
184 
185  for (unsigned int i = frame.renders.boundingBox.getTop(); i < frame.renders.boundingBox.getBottom(); i += 2) {
186  for (unsigned int j = frame.renders.boundingBox.getLeft(); j < frame.renders.boundingBox.getRight(); j += 2) {
187  double u = static_cast<double>(j), v = static_cast<double>(i);
188  double x = 0.0, y = 0.0;
189  double Z = frame.renders.depth[i][j];
190  if (Z > 0.f) {
191  vpPixelMeterConversion::convertPoint(frame.cam, u, v, x, y);
192  cX[0] = x * Z;
193  cX[1] = y * Z;
194  cX[2] = Z;
195  oX = oMcprev * cX;
196  vpStored3DSurfaceColorPoint newPoint;
197  newPoint.X[0] = oX[0] / oX[3];
198  newPoint.X[1] = oX[1] / oX[3];
199  newPoint.X[2] = oX[2] / oX[3];
200  const vpRGBa &c = previousFrame.IRGB[i][j];
201  const float colorVariance = std::pow(static_cast<float>(m_initialColorSigma), 2);
202  newPoint.stats.init(vpRGBf(c.R, c.G, c.B), vpRGBf(colorVariance));
203  bool canAdd = true;
204  for (const vpStored3DSurfaceColorPoint &p : m_points) {
205  if (p.squaredDist(newPoint.X) < vpMath::sqr(m_minDist3DNewPoint)) {
206  canAdd = false;
207  break;
208  }
209  }
210  if (canAdd) {
211  m_points.push_back(newPoint);
212  }
213  }
214  }
215  }
216 }
217 
219 {
220  // for (const vpStored3DSurfaceColorPoint &p : m_points) {
221  // if (p.visible) {
222  // const vpRGBf color = p.stats.mean;
223  // // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor::blue);
224  // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor(color.R, color.G, color.B), 3);
225  // // vpDisplay::displayPoint(I, p.projCurrPx[1], p.projCurrPx[0], vpColor::white, 3);
226 
227  // // vpDisplay::displayLine(I, p.projCurrPx[1], p.projCurrPx[0], p.projPrevPx[1], p.projPrevPx[0], vpColor::red);
228  // }
229  // }
230 }
231 
233 {
234  return m_score;
235 }
236 
238 {
239  return m_score < 0.2;
240 }
241 
242 #if defined(VISP_HAVE_NLOHMANN_JSON)
244 {
245  setColorUpdateRate(j.value("colorUpdateRate", m_colorUpdateRate));
246  setDepthStandardDeviation(j.value("depthSigma", m_depthSigma));
247  setFilteringMax3DError(j.value("filteringMaxDistance", m_maxError3D));
248  setInitialColorStandardDeviation(j.value("initialColorSigma", m_initialColorSigma));
249  setMinDistForNew3DPoints(j.value("minDistanceNewPoints", m_minDist3DNewPoint));
250 }
251 #endif
252 
253 END_VISP_NAMESPACE
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static double dotProd(const vpColVector &a, const vpColVector &b)
vpColVector & normalize()
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static double rad(double deg)
Definition: vpMath.h:129
static double sqr(double x)
Definition: vpMath.h:203
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
All the data related to a single tracking frame. This contains both the input data (from a real camer...
vpImage< vpRGBa > IRGB
Image luminance.
std::vector< vpRBSilhouettePoint > silhouettePoints
vpImage< float > depth
RGB image, 0 sized if RGB is not available.
vpRBRenderData renders
camera parameters
double getScore() const VP_OVERRIDE
Returns the probability [0, 1] that tracking is successful.
void setColorUpdateRate(double updateRate)
Set the update rate for the color distribution. It should be between 0 and 1.
void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE
void display(const vpImage< vpRGBa > &I) VP_OVERRIDE
Displays the information used for drift detection.
bool hasDiverged() const VP_OVERRIDE
Returns whether the tracking has diverged and should be reinitialized. This function should be called...
void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) VP_OVERRIDE
Update the algorithm after a new tracking step.
Silhouette point simple candidate representation.
Definition: vpRGBa.h:70
unsigned char B
Blue component.
Definition: vpRGBa.h:187
unsigned char R
Red component.
Definition: vpRGBa.h:185
unsigned char G
Green component.
Definition: vpRGBa.h:186
Definition: vpRGBf.h:64
float B
Blue component.
Definition: vpRGBf.h:157
float G
Green component.
Definition: vpRGBf.h:156
float R
Red component.
Definition: vpRGBf.h:155
double getLeft() const
Definition: vpRect.h:173
double getRight() const
Definition: vpRect.h:179
double getBottom() const
Definition: vpRect.h:97
double getTop() const
Definition: vpRect.h:192
Class that consider the case of a translation vector.
vpImage< float > depth
Image containing the per-pixel normal vector (RGB, in object space)
vpHomogeneousMatrix cMo
vpImage< vpRGBf > normals