#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_CATCH2)
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/rbt/vpRBTracker.h>
#include <visp3/rbt/vpRBSilhouetteMeTracker.h>
#include <visp3/rbt/vpRBSilhouetteCCDTracker.h>
#include <visp3/rbt/vpRBKltTracker.h>
#include <visp3/rbt/vpRBDenseDepthTracker.h>
#include <visp3/ar/vpPanda3DFrameworkManager.h>
#include "test_utils.h"
#if defined(VISP_HAVE_NLOHMANN_JSON)
#include VISP_NLOHMANN_JSON(json.hpp)
#endif
#define CATCH_CONFIG_RUNNER
#include <catch_amalgamated.hpp>
#ifdef ENABLE_VISP_NAMESPACE
#endif
const std::string objCube =
"o Cube\n"
"v -0.050000 -0.050000 0.050000\n"
"v -0.050000 0.050000 0.050000\n"
"v -0.050000 -0.050000 -0.050000\n"
"v -0.050000 0.050000 -0.050000\n"
"v 0.050000 -0.050000 0.050000\n"
"v 0.050000 0.050000 0.050000\n"
"v 0.050000 -0.050000 -0.050000\n"
"v 0.050000 0.050000 -0.050000\n"
"vn -1.0000 -0.0000 -0.0000\n"
"vn -0.0000 -0.0000 -1.0000\n"
"vn 1.0000 -0.0000 -0.0000\n"
"vn -0.0000 -0.0000 1.0000\n"
"vn -0.0000 -1.0000 -0.0000\n"
"vn -0.0000 1.0000 -0.0000\n"
"vt 0.375000 0.000000\n"
"vt 0.375000 1.000000\n"
"vt 0.125000 0.750000\n"
"vt 0.625000 0.000000\n"
"vt 0.625000 1.000000\n"
"vt 0.875000 0.750000\n"
"vt 0.125000 0.500000\n"
"vt 0.375000 0.250000\n"
"vt 0.625000 0.250000\n"
"vt 0.875000 0.500000\n"
"vt 0.375000 0.750000\n"
"vt 0.625000 0.750000\n"
"vt 0.375000 0.500000\n"
"vt 0.625000 0.500000\n"
"s 0\n"
"f 2/4/1 3/8/1 1/1/1\n"
"f 4/9/2 7/13/2 3/8/2\n"
"f 8/14/3 5/11/3 7/13/3\n"
"f 6/12/4 1/2/4 5/11/4\n"
"f 7/13/5 1/3/5 3/7/5\n"
"f 4/10/6 6/12/6 8/14/6\n"
"f 2/4/1 4/9/1 3/8/1\n"
"f 4/9/2 8/14/2 7/13/2\n"
"f 8/14/3 6/12/3 5/11/3\n"
"f 6/12/4 2/5/4 1/2/4\n"
"f 7/13/5 5/11/5 1/3/5\n"
"f 4/10/6 2/6/6 6/12/6\n";
SCENARIO("Instantiating a silhouette me tracker", "[rbt]")
{
GIVEN("A base me tracker")
{
WHEN("Changing mask parameters")
{
THEN("Enabling mask is seen")
{
}
THEN("Changing mask min confidence with a correct value is Ok")
{
}
THEN("Setting incorrect mask confidence value fails")
{
}
}
WHEN("Changing robust threshold")
{
THEN("Setting correct value works")
{
}
THEN("Setting negative value throws")
{
}
}
WHEN("Changing number of candidates")
{
THEN("Setting correct value works")
{
}
THEN("Setting incorrect value throws")
{
}
}
WHEN("Changing convergence settings")
{
THEN("Setting correct single point value works")
{
}
THEN("Setting incorrect single point value throws")
{
}
THEN("Setting correct global value works")
{
}
}
#if defined(VISP_HAVE_NLOHMANN_JSON)
WHEN("defining JSON parameters")
{
nlohmann::json j = {
{"type", "silhouetteMe"},
{ "numCandidates", 1 },
{ "weight", 0.5 },
{ "convergencePixelThreshold", 0.5 },
{ "convergenceRatio", 0.99},
{ "useMask", true},
{ "minMaskConfidence", 0.5},
{ "movingEdge", {
{"maskSign", 0},
{"maskSize" , 5},
{"minSampleStep" , 4.0},
{"mu" , {0.5, 0.5}},
{"nMask" , 90},
{"ntotalSample" , 0},
{"pointsToTrack" , 200},
{"range" , 5},
{"sampleStep" , 4.0},
{"strip" , 2},
{"thresholdType" , "normalized"},
{"threshold" , 20.0}
}}
};
THEN("Loading correct settings works")
{
}
THEN("Setting incorrect candidate number throws")
{
j["numCandidates"] = 0;
}
THEN("Setting incorrect mask confidence throws")
{
j["minMaskConfidence"] = 5.0;
}
THEN("Setting incorrect single point convergence vlaue confidence throws")
{
j["convergencePixelThreshold"] = -1.0;
}
THEN("Setting incorrect global convergence vlaue confidence throws")
{
j["convergenceRatio"] = 2.0;
}
}
}
#endif
}
SCENARIO("Instantiating a silhouette CCD tracker", "[rbt]")
{
WHEN("Setting smoothing factor")
{
THEN("Setting value above 0 works")
{
}
THEN("Setting value below 0 throws")
{
}
}
WHEN("Updating CCD parameters")
{
THEN("Changes are propagated to tracker")
{
}
}
#if defined(VISP_HAVE_NLOHMANN_JSON)
WHEN("Defining associated json")
{
nlohmann::json j = {
{"type", "silhouetteCCD"},
{"weight", 0.01},
{"temporalSmoothing", 0.1},
{"convergenceThreshold", 0.1},
{"ccd", {
{"h", 64},
{"delta_h", 16},
{"gamma", { 0.1, 0.2, 0.3, 0.4 } }
}}
};
THEN("Loading correct json works")
{
}
THEN("Loading invalid temporal smoothing factor throws")
{
j["temporalSmoothing"] = -3.14;
}
THEN("Loading invalid ccd gamma throws")
{
j["ccd"]["gamma"] = -3.14;
}
}
#endif
}
#if defined(VP_HAVE_RB_KLT_TRACKER)
SCENARIO("Instantiating KLT tracker")
{
vpRBKltTracker tracker;
WHEN("Modifying basic settings")
{
tracker.setFilteringBorderSize(2);
tracker.setFilteringMaxReprojectionError(0.024);
tracker.setMinimumDistanceNewPoints(0.005);
tracker.setMinimumNumberOfPoints(20);
tracker.setMinimumMaskConfidence(0.5);
THEN("Every change is visible")
{
REQUIRE(tracker.getFilteringBorderSize() == 2);
REQUIRE(tracker.getFilteringMaxReprojectionError() == 0.024);
REQUIRE(tracker.getMinimumDistanceNewPoints() == 0.005);
REQUIRE(tracker.getMinimumNumberOfPoints() == 20);
REQUIRE(tracker.shouldUseMask());
REQUIRE(tracker.getMinimumMaskConfidence() == 0.5);
}
THEN("Setting incorrect Mask confidence throws")
{
REQUIRE_THROWS(tracker.setMinimumMaskConfidence(-1.0));
}
}
#if defined(VISP_HAVE_NLOHMANN_JSON)
WHEN("Defining associated json")
{
nlohmann::json j = {
{"type", "klt"},
{"weight", 0.01},
{"minimumNumPoints", 25},
{"newPointsMinPixelDistance", 5},
{"maxReprojectionErrorPixels", 0.01},
{"useMask", true},
{"minMaskConfidence", 0.1},
{ "windowSize", 7 },
{ "quality", 0.01 },
{ "maxFeatures", 500 }
};
THEN("Loading correct json works")
{
tracker.loadJsonConfiguration(j);
REQUIRE(tracker.getMinimumNumberOfPoints() == 25);
REQUIRE(tracker.getMinimumDistanceNewPoints() == 5);
REQUIRE(tracker.getFilteringMaxReprojectionError() == 0.01);
REQUIRE(tracker.shouldUseMask() == true);
REQUIRE(tracker.getMinimumMaskConfidence() == 0.1f);
REQUIRE(tracker.getKltTracker().getWindowSize() == 7);
REQUIRE(tracker.getKltTracker().getQuality() == 0.01);
REQUIRE(tracker.getKltTracker().getMaxFeatures() == 500);
}
THEN("Loading invalid mask confidence throws")
{
j["minMaskConfidence"] = -3.14;
REQUIRE_THROWS(tracker.loadJsonConfiguration(j));
}
}
#endif
}
#endif
SCENARIO("Instantiating depth tracker", "[rbt]")
{
WHEN("Setting steps")
{
THEN("Setting positive value works")
{
}
THEN("Setting 0 step is invalid")
{
REQUIRE_THROWS(tracker.
setStep(0));
}
}
WHEN("Setting confidence")
{
THEN("Setting incorrect mask confidence value")
{
}
THEN("Setting correct mask confidence value")
{
}
THEN("Toggling mask works")
{
}
}
#if defined(VISP_HAVE_NLOHMANN_JSON)
WHEN("Defining associated json")
{
nlohmann::json j = {
{"type", "klt"},
{"weight", 0.01},
{"step", 16},
{"useMask", true},
{"minMaskConfidence", 0.1}
};
THEN("Loading correct json works")
{
}
THEN("Loading invalid mask confidence throws")
{
j["minMaskConfidence"] = -3.14;
}
THEN("Loading invalid step throws")
{
j["step"] = 0;
}
}
#endif
}
SCENARIO("Instantiating a render-based tracker", "[rbt]")
{
WHEN("Setting optimization parameters")
{
THEN("Max num iter cannot be zero")
{
}
THEN("Setting num iter is ok")
{
}
THEN("Gain cannot be negative")
{
}
THEN("Positive gain is ok")
{
}
THEN("Initial mu cannot be negative")
{
}
THEN("Initial mu can be zero (gauss newton)")
{
}
THEN("Initial mu can be above zero")
{
}
THEN("Mu factor cannot be negative")
{
}
THEN("Mu factor can be zero")
{
}
THEN("Mu factor can be positive")
{
}
}
WHEN("Setting camera parameters and resolution")
{
unsigned int h = 480, w = 640;
THEN("Image height cannot be zero")
{
}
THEN("Image width cannot be zero")
{
}
THEN("Camera model cannot have distortion")
{
cam.initPersProjWithDistortion(600, 600, 320, 240, 0.01, 0.01);
}
THEN("Loading with perspective model with no distortion and correct resolution is ok")
{
}
}
#if defined(VISP_HAVE_NLOHMANN_JSON)
WHEN("Loading JSON configuration")
{
const std::string jsonLiteral = R"JSON({
"camera": {
"intrinsics": {
"model": "perspectiveWithoutDistortion",
"px" : 302.573,
"py" : 302.396,
"u0" : 162.776,
"v0" : 122.475
},
"height": 240,
"width" : 320
},
"vvs": {
"gain": 1.0,
"maxIterations" : 10,
"mu": 0.5,
"muIterFactor": 0.1
},
"model" : "path/to/model.obj",
"silhouetteExtractionSettings" : {
"threshold": {
"type": "relative",
"value" : 0.1
},
"sampling" : {
"type": "fixed",
"samplingRate": 2,
"numPoints" : 128,
"reusePreviousPoints": true
}
},
"features": [
{
"type": "silhouetteMe",
"weight" : 0.5,
"numCandidates" : 3,
"convergencePixelThreshold" : 3,
"convergenceRatio" : 0.99,
"movingEdge" : {
"maskSign": 0,
"maskSize" : 5,
"minSampleStep" : 4.0,
"mu" : [
0.5,
0.5
] ,
"nMask" : 90,
"ntotalSample" : 0,
"pointsToTrack" : 200,
"range" : 5,
"sampleStep" : 4.0,
"strip" : 2,
"thresholdType" : "normalized",
"threshold" : 20.0
}
},
{
"type": "silhouetteColor",
"weight" : 0.5,
"convergenceThreshold" : 0.1,
"temporalSmoothing" : 0.1,
"ccd" : {
"h": 4,
"delta_h" : 1
}
}
],
"verbose": {
"enabled": true
}
})JSON";
const auto verifyBase = [&tracker]() {
};
nlohmann::json j = nlohmann::json::parse(jsonLiteral);
THEN("Loading configuration with trackers")
{
verifyBase();
AND_THEN("Initializing tracking fails since object does not exist")
{
}
}
THEN("Loading configuration without model also works")
{
j.erase("model");
verifyBase();
AND_THEN("Initializing tracking fails since path is not specified")
{
}
}
THEN("Loading configuration with real 3D model also works")
{
std::ofstream f(objFile);
f << objCube;
f.close();
j["model"] = objFile;
verifyBase();
AND_THEN("Initializing tracker works")
{
}
}
}
WHEN("Adding trackers")
{
THEN("Adding nullptr is not allowed")
{
}
THEN("Adding a tracker works")
{
auto ccdTracker = std::make_shared<vpRBSilhouetteCCDTracker>();
}
}
#endif
}
SCENARIO("Running tracker on static synthetic sequences", "[rbt]")
{
unsigned int h = 480, w = 640;
std::cout << tempDir << std::endl;
std::ofstream f(objFile);
f << objCube;
f.close();
renderer.addNodeToScene(renderer.loadObject("object", objFile));
};
const unsigned int n = 100;
std::vector<vpHomogeneousMatrix> cTw;
std::vector<vpHomogeneousMatrix> oTw;
for (unsigned int i = 0; i < n; ++i) {
cTw.push_back(
vpHomogeneousMatrix(0.0, 0.001 *
static_cast<double>(i), 0.3 + 0.001 *
static_cast<double>(i), 0.0, 0.0, 0.0));
}
TrajectoryData traj1 = generateTrajectory(renderParams, setupScene, cTw, oTw);
std::shared_ptr<vpRBSilhouetteCCDTracker> silTracker = std::make_shared<vpRBSilhouetteCCDTracker>();
silTracker->setTemporalSmoothingFactor(0.1);
silTracker->setCCDParameters(ccdParams);
for (
unsigned int i = 0; i < traj1.
cTo.size(); ++i) {
std::cout <<
"Translation error = " << errorT <<
" m" <<
", rotation error = " <<
vpMath::deg(errorR) <<
" deg" << std::endl;
REQUIRE((errorT < 0.005 && errorR <
vpMath::deg(2.1)));
}
}
int main(int argc, char *argv[])
{
Catch::Session session;
session.applyCommandLine(argc, argv);
int numFailed = session.run();
return numFailed;
}
#else
int main()
{
return EXIT_SUCCESS;
}
#endif
double gamma_2
Curve uncertainty computation hyperparameter Recommended to leave fixed.
double gamma_3
Curve uncertainty computation hyperparameter Recommended to leave fixed.
double gamma_1
Curve uncertainty computation hyperparameter Recommended to leave fixed.
int delta_h
Sample step when computing statistics and errors. Increase this value to decrease computation time,...
int h
Size of the vicinity that is used to compute statistics and error. Length of the line along the norma...
double gamma_4
Curve uncertainty computation hyperparameter Recommended to leave fixed.
Generic class defining intrinsic camera parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
vpTranslationVector getTranslationVector() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static double rad(double deg)
static double deg(double rad)
unsigned int getMaskNumber() const
double getThreshold() const
Class representing an ambient light.
static vpPanda3DFrameworkManager & getInstance()
Rendering parameters for a panda3D simulation.
Class that renders multiple datatypes, in a single pass. A renderer set contains multiple subrenderer...
A tracker based on dense depth point-plane alignment.
void setMinimumMaskConfidence(float confidence)
virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE
unsigned int getStep() const
void setStep(unsigned int step)
bool shouldUseMask() const
Returns whether the tracking algorithm should filter out points that are unlikely to be on the object...
float getMinimumMaskConfidence() const
Returns the minimum mask confidence that a pixel linked to depth point should have if it should be ke...
void setShouldUseMask(bool useMask)
Tracking based on the Contracting Curve Density algorithm.
void setTemporalSmoothingFactor(double factor)
Sets the temporal smoothing factor.
void setCCDParameters(const vpCCDParameters ¶meters)
double getTemporalSmoothingFactor() const
Returns the amount of temporal smoothing applied when computing the tracking error and its jacobian....
virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE
void setShouldUseMask(bool useMask)
vpCCDParameters getCCDParameters() const
Moving edge feature tracking from depth-extracted object contours.
virtual void loadJsonConfiguration(const nlohmann::json &j) VP_OVERRIDE
void setNumCandidates(unsigned int candidates)
double getSinglePointConvergenceThreshold() const
void setMinimumMaskConfidence(float confidence)
float getMinimumMaskConfidence() const
Returns the minimum mask confidence that a pixel linked to depth point should have if it should be ke...
void setSinglePointConvergenceThreshold(double threshold)
bool shouldUseMask() const
Returns whether the tracking algorithm should filter out points that are unlikely to be on the object...
double getGlobalConvergenceMinimumRatio() const
void setGlobalConvergenceMinimumRatio(double threshold)
double getMinRobustThreshold() const
const vpMe & getMe() const
void setShouldUseMask(bool useMask)
unsigned int getNumCandidates() const
void setMinRobustThreshold(double threshold)
void track(const vpImage< unsigned char > &I)
double getOptimizationInitialMu() const
unsigned int getImageWidth() const
vpCameraParameters getCameraParameters() const
void setModelPath(const std::string &path)
double getOptimizationMuIterFactor() const
void getPose(vpHomogeneousMatrix &cMo) const
void loadConfiguration(const nlohmann::json &j)
void addTracker(std::shared_ptr< vpRBFeatureTracker > tracker)
unsigned int getImageHeight() const
double getOptimizationGain() const
void setSilhouetteExtractionParameters(const vpSilhouettePointsExtractionSettings &settings)
unsigned int getMaxOptimizationIters() const
void setOptimizationMuIterFactor(double factor)
void setOptimizationInitialMu(double mu)
void setOptimizationGain(double lambda)
std::string getModelPath() const
void setPose(const vpHomogeneousMatrix &cMo)
vpSilhouettePointsExtractionSettings getSilhouetteExtractionParameters() const
void setMaxOptimizationIters(unsigned int iters)
void setCameraParameters(const vpCameraParameters &cam, unsigned h, unsigned w)
double frobeniusNorm() const
std::vector< vpHomogeneousMatrix > cTo
std::vector< vpImage< float > > depth
std::vector< vpImage< vpRGBa > > rgb