vpRBProbabilistic3DDriftDetector.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

 
#ifndef VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
#define VP_RB_PROBABILISTIC_3D_DRIFT_DETECTOR_H
 
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpRGBf.h>
#include <visp3/core/vpRGBa.h>
 
#include <visp3/rbt/vpRBDriftDetector.h>
 
#include <array>
 
BEGIN_VISP_NAMESPACE
 
template <typename T> class vpImage;

class VISP_EXPORT vpRBProbabilistic3DDriftDetector : public vpRBDriftDetector
{
public:
 
  struct vpStored3DSurfaceColorPoint
  {

    struct ColorStatistics
    {
      ColorStatistics() = default;
 
      void init(const vpRGBf &c, const vpRGBf &var)
      {
        mean = c;
        variance = var;
        initVariance = var;
        // computeStddev();
      }

      void update(const vpRGBf &c, float weight)
      {
        const vpRGBf diff(c.R - mean.R, c.G - mean.G, c.B - mean.B);
        const vpRGBf diffSqr(diff.R * diff.R, diff.G * diff.G, diff.B * diff.B);
        mean = mean + weight * diff;
        variance = (1 - weight) * (variance + weight * diffSqr);
        // computeStddev();
      }

      double probability(const vpRGBf &c)
      {
        vpRGBf diff(c.R - mean.R, c.G - mean.G, c.B - mean.B);
        diff.R = (diff.R * (1.f / sqrt(initVariance.R)));
        diff.G = (diff.G * (1.f / sqrt(initVariance.G)));
        diff.B = (diff.B * (1.f / sqrt(initVariance.B)));
 
 
        // const double dist = sqrt(diff.R + diff.G + diff.B);
 
        // const double dist = sqrt(
        //   std::pow((c.R - mean.R) / (standardDev.R), 2) +
        //   std::pow((c.G - mean.G) / (standardDev.G), 2) +
        //   std::pow((c.B - mean.B) / (standardDev.B), 2));
 
        // const double proba = 1.0 - erf(dist / sqrt(2));
 
        const double proba = 1.0 - erf(std::max(std::max(abs(diff.R), abs(diff.G)), abs(diff.B)) / sqrt(2));
        // const double proba = 1.0 - erf(dist / sqrt(2));
 
 
        return proba;
      }
 
      double trace()
      {
        return static_cast<double>(variance.R + variance.G + variance.B);
      }
 
      // void computeStddev()
      // {
      //   standardDev.R = sqrt(variance.R);
      //   standardDev.G = sqrt(variance.G);
      //   standardDev.B = sqrt(variance.B);
      // }
 
      double covarianceScaleFactor() const
      {
        // Compute the scale factor as the first eigenvalue of the covariance matrix
        // Since our matrix is diagonal, it is the heighest variance value
 
        return std::max(variance.R, std::max(variance.G, variance.B));
      }
 
      vpRGBf mean;
      vpRGBf variance;
      vpRGBf initVariance;
    };
 
    inline void update(const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &renderTo, const vpCameraParameters &cam)
    {
      fastProjection(cTo, cam, currX, projCurr, projCurrPx);
      fastProjection(renderTo, cam, renderX, projRender, projRenderPx);
    }
 
    inline double squaredDist(const std::array<double, 3> &p) const
    {
      return std::pow(p[0] - X[0], 2) + std::pow(p[1] - X[1], 2) + std::pow(p[2] - X[2], 2);
    }
 
    inline void fastProjection(const vpHomogeneousMatrix &cTo, const vpCameraParameters &cam,
      std::array<double, 3> &pC, std::array<double, 2> &proj, std::array<int, 2> &px)
    {
      const double *T = cTo.data;
      pC[0] = (T[0] * X[0] + T[1] * X[1] + T[2] * X[2] + T[3]);
      pC[1] = (T[4] * X[0] + T[5] * X[1] + T[6] * X[2] + T[7]);
      pC[2] = (T[8] * X[0] + T[9] * X[1] + T[10] * X[2] + T[11]);
      proj[0] = pC[0] / pC[2];
      proj[1] = pC[1] / pC[2];
      px[0] = static_cast<int>((proj[0] * cam.get_px()) + cam.get_u0());
      px[1] = static_cast<int>((proj[1] * cam.get_py()) + cam.get_v0());
    }
 
    void updateColor(const vpRGBf &currentColor, float updateRate)
    {
      stats.update(currentColor, updateRate);
    }
 
    vpRGBa getDisplayColor() const
    {
      return vpRGBa(static_cast<unsigned int>(stats.mean.R), static_cast<unsigned int>(stats.mean.G), static_cast<unsigned int>(stats.mean.B));
    }
 
    std::array<double, 3> X; // Point position in object frame
    ColorStatistics stats; 
    std::array<double, 3> currX, renderX; 
    std::array<double, 2> projCurr, projRender; // Projection in camera normalized coordinates of the point for the current and previous camera poses.
    std::array<int, 2> projCurrPx, projRenderPx; // Projection in pixels of the point for the current and previous camera poses.
    bool visible; // Whether the point is visible
  };
 
public:
 
  vpRBProbabilistic3DDriftDetector() : m_colorUpdateRate(0.2), m_initialColorSigma(25.0), m_depthSigma(0.04), m_maxError3D(0.001), m_minDist3DNewPoint(0.003), m_sampleStep(4)
  { }
 
  void update(const vpRBFeatureTrackerInput &previousFrame, const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo, const vpHomogeneousMatrix &cprevTo) VP_OVERRIDE;
 
  double score(const vpRBFeatureTrackerInput &frame, const vpHomogeneousMatrix &cTo) VP_OVERRIDE;
 
 
 

  double getScore() const VP_OVERRIDE;
 
  bool hasDiverged() const VP_OVERRIDE;
 
  void reset() VP_OVERRIDE
  {
    m_points.clear();
  }
 
  void display(const vpImage<vpRGBa> &I) VP_OVERRIDE;


  double getMinDistForNew3DPoints() const { return m_minDist3DNewPoint; }
 
  void setMinDistForNew3DPoints(double distance)
  {
    if (distance <= 0.0) {
      throw vpException(vpException::badValue, "Distance criterion for candidate rejection should be greater than 0.");
    }
    m_minDist3DNewPoint = distance;
  }

  double getFilteringMax3DError() const { return m_maxError3D; }
 
  void setFilteringMax3DError(double maxError)
  {
    if (maxError <= 0.0) {
      throw vpException(vpException::badValue, "Maximum 3D error for rejection should be greater than 0.");
    }
    m_maxError3D = maxError;
  }

  double getDepthStandardDeviation() const { return m_depthSigma; }
  void setDepthStandardDeviation(double sigma)
  {
    if (sigma < 0.0) {
      throw vpException(vpException::badValue, "Depth standard deviation should be greater than 0");
    }
    m_depthSigma = sigma;
  }

  double getInitialColorStandardDeviation() const { return m_depthSigma; }
  void setInitialColorStandardDeviation(double sigma)
  {
    if (sigma < 0.0) {
      throw vpException(vpException::badValue, "Initial color standard deviation should be greater than 0");
    }
    m_initialColorSigma = sigma;
  }

  double getColorUpdateRate() const { return m_colorUpdateRate; }

  void setColorUpdateRate(double updateRate)
  {
    if (updateRate < 0.0 || updateRate > 1.f) {
      throw vpException(vpException::badValue, "Color update rate should be between 0 and 1");
    }
    m_colorUpdateRate = updateRate;
  }
 
  unsigned int getSampleStep() const { return m_sampleStep; }
  void setSampleStep(unsigned int sampleStep)
  {
    if (sampleStep == 0) {
      throw vpException(vpException::badValue, "Image sample step should be greater than 0");
    }
    m_sampleStep = sampleStep;
  }
 
#if defined(VISP_HAVE_NLOHMANN_JSON)
  void loadJsonConfiguration(const nlohmann::json &) VP_OVERRIDE;
  void loadRepresentation(const std::string &);
  void saveRepresentation(const std::string &) const;
#endif

 
private:
  double m_colorUpdateRate;
  double m_initialColorSigma;
  double m_depthSigma;
  double m_maxError3D;
  double m_minDist3DNewPoint;
  unsigned int m_sampleStep;
 
  double m_score;
 
  std::vector<vpStored3DSurfaceColorPoint> m_points;
};
 
#ifdef VISP_HAVE_NLOHMANN_JSON
inline void from_json(const nlohmann::json &j, vpRBProbabilistic3DDriftDetector::vpStored3DSurfaceColorPoint::ColorStatistics &c)
{
  c.mean = j.at("mean").get<vpRGBf>();
  c.variance = j.at("variance").get<vpRGBf>();
  // c.computeStddev();
}
 
inline void to_json(nlohmann::json &j, const vpRBProbabilistic3DDriftDetector::vpStored3DSurfaceColorPoint::ColorStatistics &c)
{
  j["mean"] = c.mean;
  j["variance"] = c.variance;
}
 
inline void from_json(const nlohmann::json &j, vpRBProbabilistic3DDriftDetector::vpStored3DSurfaceColorPoint &p)
{
  p.X = j.at("X");
  p.stats = j.at("stats");
}
 
inline void to_json(nlohmann::json &j, const vpRBProbabilistic3DDriftDetector::vpStored3DSurfaceColorPoint &p)
{
  j["X"] = p.X;
  j["stats"] = p.stats;
}
 
 
 
#endif
 
END_VISP_NAMESPACE
 
#endif