#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpUniRand.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpImageSimulator.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/visual_features/vpFeatureLuminanceMapping.h>
#include <stdlib.h>
#ifdef ENABLE_VISP_NAMESPACE
#endif
#define GETOPTARGS "cdi:n:p:m:k:hl:"
void usage(const char *name, const char *badparam, std::string ipath, int niter, const std::string &method, unsigned numDbImages, const unsigned numComponents, const double lambda);
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents, double &lambda);
void usage(const char *name, const char *badparam, std::string ipath, int niter, const std::string &method, unsigned numDbImages, const unsigned numComponents, const double lambda)
{
fprintf(stdout, "\n\
Visual servoing with compressed photometric features.\n\
Use either PCA or DCT representations\n\
\n\
\n\
SYNOPSIS\n\
%s [-i <input image path>] [-m pca|dct] [-p <v>] [-c] [-d] [-n <number of iterations>] [-h]\n",
name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read \"doisneau/doisneau.jpg\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
\n\
-m\n\
Method to use: either 'PCA' or 'DCT'\n\
PCA first requires learning a projection from a base of images. see the -p option.\n\
Default: %s\n\
-k\n\
Number of visual servoing features (i.e., PCA or DCT components)\n\
Default: %d\n\
\n\
-p\n\
Number of images to use to compute PCA. If method is DCT, this option is ignored.\n\
Default: %d\n\
\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-n %%d %d\n\
Number of visual servoing iterations.\n\
\n\
-l %%f %f\n\
Number of visual servoing iterations.\n\
\n\
-h\n\
Print the help.\n",
ipath.c_str(), method.c_str(), numComponents, numDbImages, niter, lambda);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display,
int &niter, std::string &method, unsigned &numDbImages, unsigned &numComponents, double &lambda)
{
const char *optarg_;
int c;
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'i':
ipath = optarg_;
break;
case 'm':
method = std::string(optarg_);
break;
case 'p':
numDbImages = atoi(optarg_);
break;
case 'k':
numComponents = atoi(optarg_);
break;
case 'n':
niter = atoi(optarg_);
break;
case 'l':
lambda = atof(optarg_);
break;
case 'h':
usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents, lambda);
return false;
default:
usage(argv[0], optarg_, ipath, niter, method, numDbImages, numComponents, lambda);
return false;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], nullptr, ipath, niter, method, numDbImages, numComponents, lambda);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int main(int argc, const char **argv)
{
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
try {
std::string env_ipath;
std::string opt_ipath;
std::string ipath;
std::string filename;
bool opt_click_allowed = true;
bool opt_display = true;
int opt_niter = 400;
std::string opt_method = "dct";
unsigned opt_numDbImages = 2000;
unsigned opt_numComponents = 32;
double opt_lambda = 5.0;
double mu = 0.01;
double lambdaGN = opt_lambda;
const double Z = 0.8;
const unsigned ih = 240;
const unsigned iw = 320;
const double scenew = 0.6;
const double sceneh = 0.42;
if (!env_ipath.empty())
ipath = env_ipath;
if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter, opt_method,
opt_numDbImages, opt_numComponents, opt_lambda) == false) {
return EXIT_FAILURE;
}
if (!opt_ipath.empty())
ipath = opt_ipath;
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], nullptr, ipath, opt_niter, opt_method, opt_numDbImages, opt_numComponents, opt_lambda);
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
return EXIT_FAILURE;
}
for (int i = 0; i < 4; i++)
X[i].resize(3);
X[0][0] = -(scenew / 2.0);
X[0][1] = -(sceneh / 2.0);
X[0][2] = 0;
X[1][0] = (scenew / 2.0);
X[1][1] = -(sceneh / 2.0);
X[1][2] = 0;
X[2][0] = (scenew / 2.0);
X[2][1] = (sceneh / 2.0);
X[2][2] = 0;
X[3][0] = -(scenew / 2.0);
X[3][1] = (sceneh / 2.0);
X[3][2] = 0;
cdMo[2][3] = Z;
std::shared_ptr<vpLuminanceMapping> sMapping = nullptr;
std::shared_ptr<vpLuminanceMapping> sdMapping = nullptr;
if (opt_method == "pca") {
std::cout << "Building image database for PCA computation with " << opt_numDbImages << " images" << std::endl;
#if defined(VISP_HAVE_GUI)
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#elif defined(HAVE_OPENCV_HIGHGUI)
#endif
if (opt_display) {
d.
init(I, 0, 0,
"Image database (subsample)");
}
#endif
std::vector<vpImage<unsigned char>> images(opt_numDbImages);
for (unsigned i = 0; i < opt_numDbImages; ++i) {
const double noiseDiv = 16.0;
positionNoise[0] = random.uniform(-scenew / noiseDiv, scenew / noiseDiv);
positionNoise[1] = random.uniform(-sceneh / noiseDiv, sceneh / noiseDiv);
positionNoise[2] = random.uniform(0.0, Z / noiseDiv);
const double noiseDivTo = 16.0;
to[0] = random.uniform(-scenew / noiseDivTo, scenew / noiseDivTo);
to[1] = random.uniform(-sceneh / noiseDivTo, sceneh / noiseDivTo);
images[i] = I;
if (i % 20 == 0 && opt_display) {
}
}
std::cout << "Computing PCA, this may take some time!" << std::endl;
sMapping = std::shared_ptr<vpLuminanceMapping>(
new vpLuminancePCA(pca));
sdMapping = std::shared_ptr<vpLuminanceMapping>(
new vpLuminancePCA(pca));
}
else if (opt_method == "dct") {
sMapping = std::shared_ptr<vpLuminanceMapping>(
new vpLuminanceDCT(opt_numComponents));
sdMapping = std::shared_ptr<vpLuminanceMapping>(
new vpLuminanceDCT(opt_numComponents));
}
else {
}
Id = I;
#if defined(VISP_HAVE_GUI)
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#elif defined(HAVE_OPENCV_HIGHGUI)
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_OPENCV)
if (opt_display) {
d.
init(I, 20, 10,
"Current image");
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
}
#endif
#endif
I = 0u;
#if defined(VISP_HAVE_GUI) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
if (opt_display) {
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
}
#endif
Idiff = I;
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#endif
if (opt_display) {
d1.
init(Idiff, 40 +
static_cast<int>(I.
getWidth()), 10,
"photometric visual servoing : s-s* ");
d2.
init(Irec, 40 +
static_cast<int>(I.
getWidth()) * 2, 10,
"Reconstructed image");
}
#endif
robot.setSamplingTime(0.04);
sI.build(I);
sI.getMapping()->inverse(sI.get_s(), Irec);
sId.build(Id);
int iter = 1;
int iterGN = opt_niter / 8;
double normError = 0;
unsigned int n = 6;
std::cout << "Starting VS loop" << std::endl;
do {
std::cout << "--------------------------------------------" << iter++ << std::endl;
sI.build(I);
sI.getMapping()->inverse(sI.get_s(), Irec);
if (iter > iterGN) {
mu = 0.0001;
opt_lambda = lambdaGN;
}
sI.interaction(L);
sI.error(sId, error);
Hs = L.AtA();
for (unsigned int i = 0; i < n; i++) {
diagHs[i][i] = Hs[i][i];
}
v = -opt_lambda * H * L.t() * error;
normError = error.sumSquare();
std::cout << " |e| = " << normError << std::endl;
std::cout <<
" |v| = " << sqrt(v.
sumSquare()) << std::endl;
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
#endif
wMc = robot.
getPosition();
} while (normError > 200 && iter < opt_niter);
std::cout <<
"Time to convergence: " << chrono.
getDurationMs() <<
" ms" << std::endl;
v = 0;
if (normError > 200) {
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
#else
(void)argc;
(void)argv;
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
Generic class defining intrinsic camera parameters.
void start(bool reset=true)
Implementation of column vector and the associated operations.
static const vpColor black
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") VP_OVERRIDE
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Class to combine luminance features (photometric servoing)
Class that defines the image luminance visual feature.
void init(unsigned int _nbr, unsigned int _nbc, double _Z)
static const int DEFAULT_BORDER
void setCameraParameters(const vpCameraParameters &_cam)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setCleanPreviousImage(const bool &clean, const vpColor &color=vpColor::white)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
unsigned int getWidth() const
unsigned int getHeight() const
static vpLuminancePCA learn(const std::vector< std::string > &imageFiles, const unsigned int projectionSize, const unsigned int imageBorder=0)
Compute a new Principal Component Analysis on set of images, stored on disk.
vpColVector getExplainedVariance() const
Get the values of explained variance by each of the eigen vectors.
static double rad(double deg)
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Class for generating random numbers with uniform probability density.