Skip to content

Commit

Permalink
Stopped using initClick from vpMbTracker
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamFlt
SamFlt committed Sep 18, 2024
1 parent 6affb4b commit aee5e20
Show file tree
Hide file tree
Showing 8 changed files with 380 additions and 122 deletions.
40 changes: 40 additions & 0 deletions modules/tracker/rbt/include/visp3/rbt/vpRBInitializationHelper.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
#ifndef VP_RB_INITIALIZATION_HELPER_H
#define VP_RB_INITIALIZATION_HELPER_H

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpCameraParameters.h>

#include <string>
#include <iostream>

template <typename T>
class vpImage;


class VISP_EXPORT vpRBInitializationHelper
{
public:
void removeComment(std::ifstream &fileId);
void savePose(const std::string &filename) const;

vpHomogeneousMatrix getPose() const { return m_cMo; }

void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; }

#ifdef VISP_HAVE_MODULE_GUI
template <typename T>
void initClick(const vpImage<T> &I, const std::string &initFile, bool displayHelp);

#endif

private:
std::string m_poseSavingFileName;

vpHomogeneousMatrix m_cMo;
vpCameraParameters m_cam;
};



#endif
2 changes: 1 addition & 1 deletion modules/tracker/rbt/include/visp3/rbt/vpRBProbabilistic3DDriftDetector.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
* 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
Expand Down
117 changes: 10 additions & 107 deletions modules/tracker/rbt/include/visp3/rbt/vpRBTracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,14 @@

#if defined(VISP_HAVE_PANDA3D)

#include <visp3/mbt/vpMbTracker.h>

#include <visp3/rbt/vpRBFeatureTracker.h>
#include <visp3/rbt/vpRBSilhouettePointsExtractionSettings.h>
#include <visp3/rbt/vpPanda3DDepthFilters.h>
#include <visp3/rbt/vpObjectCentricRenderer.h>
#include <visp3/rbt/vpRBTrackerLogger.h>

#include <visp3/core/vpDisplay.h>

class vpObjectMask;
class vpRBDriftDetector;

Expand All @@ -58,82 +58,14 @@ class vpRBDriftDetector;
#include <nlohmann/json_fwd.hpp>
#endif

// #include <visp3/core/vpUnscentedKalmanPose.h>
// class VISP_EXPORT vpRBTrackerFilter
// {
// public:
// vpRBTrackerFilter() : m_initialized(false)
// {
// double opt_stdevP0 = 1e-8;
// double opt_stdevQ = 1e-10;
// double opt_stdevR = 1e-6;
// vpMatrix Id;
// Id.eye(6);
// vpMatrix P0 = Id * opt_stdevP0 * opt_stdevP0;
// vpMatrix Q = Id * opt_stdevQ * opt_stdevQ;
// vpMatrix R = Id * opt_stdevR * opt_stdevR;
// double alphaPred = 0.01;
// vpUnscentedKalmanPose::State X0;
// Id.eye(3);
// vpMatrix R_ukfm = Id * opt_stdevR * opt_stdevR;
// vpUnscentedKalmanPose ukfm(Q, R_ukfm, std::vector<double>(3, alphaPred), X0, P0,
// vpUnscentedKalmanPose::fSE3, vpUnscentedKalmanPose::hSE3, vpUnscentedKalmanPose::phiSE3,
// vpUnscentedKalmanPose::phiinvSE3);
// m_kalman = std::shared_ptr<vpUnscentedKalmanPose>(new vpUnscentedKalmanPose(ukfm));
// }

// void filter(const vpHomogeneousMatrix &cMo, double dt)
// {
// if (!m_initialized) {
// m_kalman->setX0(cMo);
// m_cMoPrev = cMo;
// m_initialized = true;
// }
// else {
// std::ios_base::fmtflags f(std::cerr.flags());

// vpColVector v = vpExponentialMap::inverse(m_cMoPrev * cMo.inverse(), dt);
// std::cerr << "Kalman v = " << std::setprecision(4) << std::scientific << v.t() << std::endl;
// std::cerr.flags(f);
// m_kalman->filter(v, vpUnscentedKalmanPose::asPositionVector(cMo), dt);
// m_cMoPrev = cMo;
// }
// }

// void reinit(const vpHomogeneousMatrix &cMo)
// {
// m_kalman->setX0(cMo);
// m_cMoPrev = cMo;
// m_initialized = true;
// }

// vpHomogeneousMatrix getFilteredPose()
// {
// return m_kalman->getState();
// }



// private:
// std::shared_ptr<vpUnscentedKalmanPose> m_kalman;
// vpHomogeneousMatrix m_cMoPrev;
// bool m_initialized;

// };





class VISP_EXPORT vpRBTracker : public vpMbTracker
class VISP_EXPORT vpRBTracker
{
public:

vpRBTracker();

~vpRBTracker() = default;


void getPose(vpHomogeneousMatrix &cMo) const;
void setPose(const vpHomogeneousMatrix &cMo);

Expand All @@ -159,47 +91,18 @@ class VISP_EXPORT vpRBTracker : public vpMbTracker

const std::shared_ptr<const vpRBDriftDetector> getDriftDetector() const { return m_driftDetector; }

#ifdef VISP_HAVE_MODULE_GUI
void initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp);

#endif

//vpRBTrackerFilter &getFilter() { return m_filter; }
//vpRBTrackerFilter &getFilter() { return m_filter; }

#if defined(VISP_HAVE_NLOHMANN_JSON)
void loadConfigurationFile(const std::string &filename);
void loadConfiguration(const nlohmann::json &j);
#endif

virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace = 0,
const std::string &name = "") VP_OVERRIDE
{ }
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace = 0,
const std::string &name = "") VP_OVERRIDE
{ }
virtual void initFaceFromCorners(vpMbtPolygon &polygon) VP_OVERRIDE { }
virtual void initFaceFromLines(vpMbtPolygon &polygon) VP_OVERRIDE { }
virtual vpColVector getError() const VP_OVERRIDE { return vpColVector(); }
virtual vpColVector getRobustWeights() const { return vpColVector(); }
virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE
{ }
virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) VP_OVERRIDE
{ }

virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
const vpHomogeneousMatrix &cMo,
const vpCameraParameters &cam,
bool displayFullModel = false)
{
return {};
}

virtual void init(const vpImage<unsigned char> &I) VP_OVERRIDE { }
virtual void resetTracker() VP_OVERRIDE { }
virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE { }
virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo) VP_OVERRIDE { }
virtual void testTracking() VP_OVERRIDE { }
virtual void track(const vpImage<vpRGBa> &I) VP_OVERRIDE { }
virtual void computeVVSInit() VP_OVERRIDE { }
virtual void computeVVSInteractionMatrixAndResidu() VP_OVERRIDE { }
protected:

void track(vpRBFeatureTrackerInput &input);
Expand Down Expand Up @@ -231,9 +134,9 @@ class VISP_EXPORT vpRBTracker : public vpMbTracker

std::vector<std::shared_ptr<vpRBFeatureTracker>> m_trackers; //! List of trackers

// vpHomogeneousMatrix m_cMo;
vpHomogeneousMatrix m_cMo;
vpHomogeneousMatrix m_cMoPrev;
//vpCameraParameters m_cam;
vpCameraParameters m_cam;

vpRBFeatureTrackerInput m_currentFrame;
vpRBFeatureTrackerInput m_previousFrame;
Expand Down
2 changes: 1 addition & 1 deletion modules/tracker/rbt/include/visp3/rbt/vpRBTrackerLogger.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ std::ostream &operator<<(std::ostream &out, const vpRBTrackerLogger &timer)
out << "Silhouette extraction: " << timer.m_silhouetteExtractionTime << "ms" << std::endl;

out << "Trackers: " << std::endl;
for (const std::pair<int, std::vector<double>> &vvsIterData : timer.m_trackerVVSIterTimes) {
for (const std::pair<const int, std::vector<double>> vvsIterData : timer.m_trackerVVSIterTimes) {
double featTrackTime = timer.m_trackerFeatureTrackingTime.find(vvsIterData.first)->second;
double featExtractionTime = timer.m_trackerFeatureExtractionTime.find(vvsIterData.first)->second;
double initVVSTime = timer.m_trackerInitVVSTime.find(vvsIterData.first)->second;
Expand Down
Loading

0 comments on commit aee5e20

Please sign in to comment.