31 #include <visp3/rbt/vpRBProbabilistic3DDriftDetector.h>
33 #include <visp3/core/vpRect.h>
34 #include <visp3/core/vpPixelMeterConversion.h>
35 #include <visp3/core/vpDisplay.h>
37 #include <visp3/rbt/vpRBFeatureTracker.h>
39 #if defined(VISP_HAVE_NLOHMANN_JSON)
40 #include VISP_NLOHMANN_JSON(json.hpp)
52 if (m_points.size() > 0) {
54 #ifdef VISP_HAVE_OPENMP
55 #pragma omp parallel for
57 for (vpStored3DSurfaceColorPoint &p : m_points) {
58 p.update(cTo, cprevTo, frame.
cam);
62 std::vector<vpStored3DSurfaceColorPoint *> visiblePoints;
63 for (vpStored3DSurfaceColorPoint &p : m_points) {
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) {
78 float ZrenderMap = frame.
renders.
depth[p.projPrevPx[1]][p.projPrevPx[0]];
80 if (ZrenderMap == 0.f || fabs(p.prevX[2] - ZrenderMap) > m_maxError3D) {
87 vpColVector cameraRay({ t[0] - p.X[0], t[1] - p.X[1], t[2] - p.X[2] });
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)) {
110 visiblePoints.push_back(&p);
114 if (visiblePoints.size() > 0) {
125 double weightSum = 0.0;
127 for (vpStored3DSurfaceColorPoint *p : visiblePoints) {
128 double maxProba = 0.0;
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)));
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;
152 averageColor = averageColor * (1.0 / 9.0);
154 const vpRGBf c(averageColor);
156 const double probaColor = p->stats.probability(c);
157 const double proba = probaColor * probaDepth;
158 if (probaDepth > 1.f || probaDepth < 0.0) {
161 if (proba > maxProba) {
166 m_score += maxProba * weight;
168 p->updateColor(minColor, m_colorUpdateRate * probaDepth);
170 m_score /= (weightSum);
187 double u =
static_cast<double>(j), v =
static_cast<double>(i);
188 double x = 0.0, y = 0.0;
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];
201 const float colorVariance = std::pow(
static_cast<float>(m_initialColorSigma), 2);
204 for (
const vpStored3DSurfaceColorPoint &p : m_points) {
205 if (p.squaredDist(newPoint.X) <
vpMath::sqr(m_minDist3DNewPoint)) {
211 m_points.push_back(newPoint);
239 return m_score < 0.2;
242 #if defined(VISP_HAVE_NLOHMANN_JSON)
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
vpColVector & normalize()
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
unsigned int getWidth() const
unsigned int getHeight() const
static double rad(double deg)
static double sqr(double x)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
void setMinDistForNew3DPoints(double distance)
void setFilteringMax3DError(double maxError)
double getScore() const VP_OVERRIDE
Returns the probability [0, 1] that tracking is successful.
void setDepthStandardDeviation(double sigma)
void setColorUpdateRate(double updateRate)
Set the update rate for the color distribution. It should be between 0 and 1.
void setInitialColorStandardDeviation(double sigma)
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.
unsigned char B
Blue component.
unsigned char R
Red component.
unsigned char G
Green component.
Class that consider the case of a translation vector.
vpImage< float > depth
Image containing the per-pixel normal vector (RGB, in object space)
vpImage< vpRGBf > normals