Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
photometricVisualServoing.cpp

Implemented from [7].

/*
* 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.
*/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.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/io/vpParseArgv.h>
#include <visp3/visual_features/vpFeatureLuminance.h>
#include <visp3/vs/vpServo.h>
#include <stdlib.h>
#include <visp3/robot/vpImageSimulator.h>
#define Z 1
#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
#define GETOPTARGS "cdi:n:h"
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
void usage(const char *name, const char *badparam, std::string ipath, int niter);
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter);
void usage(const char *name, const char *badparam, std::string ipath, int niter)
{
fprintf(stdout, "\n\
Tracking of Surf key-points.\n\
\n\
SYNOPSIS\n\
%s [-i <input image path>] [-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\
-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 iterations.\n\
\n\
-h\n\
Print the help.\n",
ipath.c_str(), niter);
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)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'i':
ipath = optarg_;
break;
case 'n':
niter = atoi(optarg_);
break;
case 'h':
usage(argv[0], nullptr, ipath, niter);
return false;
default:
usage(argv[0], optarg_, ipath, niter);
return false;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], nullptr, ipath, niter);
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))
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;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
// environment variable value
// Set the default input path
if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_niter) == false) {
return EXIT_FAILURE;
}
// Get the option values
if (!opt_ipath.empty())
ipath = opt_ipath;
// Compare ipath and env_ipath. If they differ, we take into account
// the input path coming from the command line option
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;
}
}
// Test if an input path is set
if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], nullptr, ipath, opt_niter);
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;
}
filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImageIo::read(Itexture, filename);
for (int i = 0; i < 4; i++)
X[i].resize(3);
// Top left corner
X[0][0] = -0.3;
X[0][1] = -0.215;
X[0][2] = 0;
// Top right corner
X[1][0] = 0.3;
X[1][1] = -0.215;
X[1][2] = 0;
// Bottom right corner
X[2][0] = 0.3;
X[2][1] = 0.215;
X[2][2] = 0;
// Bottom left corner
X[3][0] = -0.3;
X[3][1] = 0.215;
X[3][2] = 0;
sim.init(Itexture, X);
vpCameraParameters cam(870, 870, 160, 120);
// ----------------------------------------------------------
// Create the framegraber (here a simulated image)
vpImage<unsigned char> I(240, 320, 0);
// camera desired position
cdMo[2][3] = 1;
// set the robot at the desired position
sim.setCameraPosition(cdMo);
sim.getImage(I, cam); // and aquire the image Id
Id = I;
// display the image
#if defined(VISP_HAVE_X11)
vpDisplayX d;
#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, "Photometric visual servoing : s");
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
}
#endif
// ----------------------------------------------------------
// position the robot at the initial position
// ----------------------------------------------------------
// camera desired position
cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20));
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
// set the robot at the desired position
I = 0u;
sim.getImage(I, cam); // and aquire the image Id
#if 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;
// Affiche de l'image de difference
#if defined(VISP_HAVE_X11)
vpDisplayX d1;
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
d1.init(Idiff, 40 + static_cast<int>(I.getWidth()), 10, "photometric visual servoing : s-s* ");
}
#endif
// create the robot (here a simulated free flying camera)
robot.setSamplingTime(0.04);
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
// ------------------------------------------------------
// Visual feature, interaction matrix, error
// s, Ls, Lsd, Lt, Lp, etc
// ------------------------------------------------------
// current visual feature built from the image
// (actually, this is the image...)
sI.init(I.getHeight(), I.getWidth(), Z);
sI.buildFrom(I);
// desired visual feature built from the image
sId.init(I.getHeight(), I.getWidth(), Z);
sId.buildFrom(Id);
// Create visual-servoing task
vpServo servo;
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// add current and desired visual features
servo.addFeature(sI, sId);
// set the gain
servo.setLambda(30);
// compute interaction matrix at the desired position
// set a velocity control mode
int iter = 1;
double normError = 0;
vpColVector v; // camera velocity send to the robot
vpChrono chrono;
chrono.start();
do {
std::cout << "--------------------------------------------" << iter++ << std::endl;
// Acquire the new image
sim.getImage(I, cam);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
#endif
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
}
#endif
// Compute current visual feature
sI.buildFrom(I);
v = servo.computeControlLaw(); // camera velocity send to the robot
normError = servo.getError().sumSquare();
std::cout << " |e| = " << normError << std::endl;
std::cout << " |v| = " << sqrt(v.sumSquare()) << std::endl;
// send the robot velocity
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
} while (normError > 10000 && iter < opt_niter);
chrono.stop();
std::cout << "Time to convergence: " << chrono.getDurationMs() << " ms" << std::endl;
v = 0;
return EXIT_SUCCESS;
}
catch (const vpException &e) {
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)
Definition: vpTime.cpp:401
void stop()
Definition: vpTime.cpp:416
double getDurationMs()
Definition: vpTime.cpp:390
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:130
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:133
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") VP_OVERRIDE
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &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.
Definition: vpException.h:60
Class that defines the image luminance visual feature.
vpFeatureLuminance & buildFrom(vpImage< unsigned char > &I)
void init(unsigned int _nbr, unsigned int _nbc, double _Z)
void setCameraParameters(const vpCameraParameters &_cam)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:147
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 setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1053
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1427
static double rad(double deg)
Definition: vpMath.h:129
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:70
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:380
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void setLambda(double c)
Definition: vpServo.h:986
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector getError() const
Definition: vpServo.h:510
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
@ CURRENT
Definition: vpServo.h:202
Class that defines the simplest robot: a free flying camera.