Visual Servoing Platform  version 3.3.0
servoSimuSphere.cpp

Demonstration of the wireframe simulator with a simple visual servoing.

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2019 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 http://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.
*
* Description:
* Demonstration of the wireframe simulator with a simple visual servoing
*
* Authors:
* Nicolas Melchior
*
*****************************************************************************/
#include <cmath> // std::fabs
#include <limits> // numeric_limits
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpSphere.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.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/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpGenericFeature.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &display);
void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s);
void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Demonstration of the wireframe simulator with a simple visual servoing.\n\
\n\
The visual servoing consists in bringing the camera at a desired position from the object.\n\
\n\
The visual features used to compute the pose of the camera and thus the control law are special moments computed with the sphere's parameters.\n\
\n\
SYNOPSIS\n\
%s [-d] [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, bool &display)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'd':
display = false;
break;
case 'h':
usage(argv[0], NULL);
return false;
break;
default:
usage(argv[0], optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
/*
Computes the virtual visual features corresponding to the sphere and stores
it in the generic feature.
The visual feature vector is computed thanks to the following formula : s =
{sx, sy, sz} sx = gx*h2/(sqrt(h2+1) sx = gy*h2/(sqrt(h2+1) sz = sqrt(h2+1)
with gx and gy the center of gravity of the ellipse,
with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
with n20,n02,n11 the second order moments of the sphere
*/
void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s)
{
double gx = sphere.get_x();
double gy = sphere.get_y();
double m02 = sphere.get_mu02();
double m20 = sphere.get_mu20();
double m11 = sphere.get_mu11();
double h2;
// if (gx != 0 || gy != 0)
if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
h2 = (vpMath::sqr(gx) + vpMath::sqr(gy)) /
(4 * m20 * vpMath::sqr(gy) + 4 * m02 * vpMath::sqr(gx) - 8 * m11 * gx * gy);
else
h2 = 1 / (4 * m20);
double sx = gx * h2 / (sqrt(h2 + 1));
double sy = gy * h2 / (sqrt(h2 + 1));
double sz = sqrt(h2 + 1); //(h2-(vpMath::sqr(sx)+vpMath::sqr(sy)-1))/(2*sqrt(h2));
s.set_s(sx, sy, sz);
}
/*
Computes the interaction matrix such as L = [-1/R*I3 [s]x]
with R the radius of the sphere
with I3 the 3x3 identity matrix
with [s]x the skew matrix related to s
*/
void computeInteractionMatrix(const vpGenericFeature &s, const vpSphere &sphere, vpMatrix &L)
{
L = 0;
L[0][0] = -1 / sphere.getR();
L[1][1] = -1 / sphere.getR();
L[2][2] = -1 / sphere.getR();
double s0, s1, s2;
s.get_s(s0, s1, s2);
vpTranslationVector c(s0, s1, s2);
sk = c.skew();
for (unsigned int i = 0; i < 3; i++)
for (unsigned int j = 0; j < 3; j++)
L[i][j + 3] = sk[i][j];
}
int main(int argc, const char **argv)
{
try {
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_display) == false) {
exit(-1);
}
vpImage<vpRGBa> Iint(480, 640, 255);
vpImage<vpRGBa> Iext1(480, 640, 255);
vpImage<vpRGBa> Iext2(480, 640, 255);
#if defined VISP_HAVE_X11
vpDisplayX display[3];
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV display[3];
#elif defined VISP_HAVE_GDI
vpDisplayGDI display[3];
#elif defined VISP_HAVE_D3D9
vpDisplayD3D display[3];
#elif defined VISP_HAVE_GTK
vpDisplayGTK display[3];
#endif
if (opt_display) {
// Display size is automatically defined by the image (I) size
display[0].init(Iint, 100, 100, "The internal view");
display[1].init(Iext1, 100, 100, "The first external view");
display[2].init(Iext2, 100, 100, "The second external view");
}
vpServo task;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
// Since the task gain lambda is very high, we need to increase default
// max velocities
// Set initial position of the object in the camera frame
vpHomogeneousMatrix cMo(0, 0.1, 2.0, vpMath::rad(35), vpMath::rad(25), 0);
// Set desired position of the object in the camera frame
vpHomogeneousMatrix cdMo(0.0, 0.0, 0.8, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// Set initial position of the object in the world frame
vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
// Position of the camera in the world frame
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
// The sphere
vpSphere sphere(0, 0, 0, 0.15);
// Projection of the sphere
sphere.track(cMo);
// Set the current visual feature
computeVisualFeatures(sphere, s);
// Projection of the points
sphere.track(cdMo);
computeVisualFeatures(sphere, sd);
vpMatrix L(3, 6);
computeInteractionMatrix(sd, sphere, L);
sd.setInteractionMatrix(L);
task.set_cVe(cVe);
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
task.addFeature(s, sd);
task.setLambda(7);
// Set the scene
// Initialize simulator frames
sim.set_fMo(wMo); // Position of the object in the world reference frame
sim.setCameraPositionRelObj(cMo); // Initial position of the object in the camera frame
sim.setDesiredCameraPosition(cdMo); // Desired position of the object in the camera frame
// Set the External camera position
vpHomogeneousMatrix camMf(0.0, 0, 3.5, vpMath::rad(0), vpMath::rad(30), 0);
// Computes the position of a camera which is fixed in the object frame
vpHomogeneousMatrix camoMf(0, 0.0, 2.5, 0, vpMath::rad(140), 0);
camoMf = camoMf * (sim.get_fMo().inverse());
// Set the parameters of the cameras (internal and external)
vpCameraParameters camera(1000, 1000, 320, 240);
int stop = 10;
if (opt_display) {
stop = 1000;
// Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
sim.getExternalImage(Iext2, camoMf);
// Display the object frame (current and desired position)
vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
// Display the object frame the world reference frame and the camera
// frame
vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo() * cMo.inverse(), camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext1, camMf * sim.get_fMo(), camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext1, camMf, camera, 0.2, vpColor::none);
// Display the world reference frame and the object frame
vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
std::cout << "Click on a display" << std::endl;
while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
!vpDisplay::getClick(Iext2, false)) {
};
}
// Print the task
task.print();
int iter = 0;
while (iter++ < stop) {
if (opt_display) {
}
double t = vpTime::measureTimeMs();
robot.get_eJe(eJe);
task.set_eJe(eJe);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
sphere.track(cMo);
// Set the current visual feature
computeVisualFeatures(sphere, s);
v = task.computeControlLaw();
// Compute the position of the external view which is fixed in the
// object frame
camoMf.buildFrom(0, 0.0, 2.5, 0, vpMath::rad(150), 0);
camoMf = camoMf * (sim.get_fMo().inverse());
if (opt_display) {
// Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
sim.getExternalImage(Iext2, camoMf);
// Display the object frame (current and desired position)
vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
// Display the camera frame, the object frame the world reference
// frame
vpDisplay::displayFrame(Iext1, sim.getExternalCameraPosition() * sim.get_fMo() * cMo.inverse(), camera, 0.2,
// Display the world reference frame and the object frame
vpDisplay::displayFrame(Iext2, camoMf, camera, 0.2, vpColor::none);
vpDisplay::displayFrame(Iext2, camoMf * sim.get_fMo(), camera, 0.05, vpColor::none);
}
vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
}
task.print();
task.kill();
return EXIT_SUCCESS;
} catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have X11, or GDI (Graphical Device Interface), or GTK functionalities to display images..." << std::endl;
std::cout << "Tip if you are on a unix-like system:" << std::endl;
std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
std::cout << "Tip if you are on a windows-like system:" << std::endl;
std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
}
#endif
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpWireFrameSimulator::SPHERE
@ SPHERE
A 15cm radius sphere.
Definition: vpWireFrameSimulator.h:192
vpWireFrameSimulator::setExternalCameraPosition
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
Definition: vpWireFrameSimulator.h:547
vpWireFrameSimulator::D_STANDARD
@ D_STANDARD
Definition: vpWireFrameSimulator.h:212
vpServo::kill
void kill()
Definition: vpServo.cpp:192
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:114
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
vpWireFrameSimulator::get_fMo
vpHomogeneousMatrix get_fMo() const
Definition: vpWireFrameSimulator.h:425
vpDisplay::setWindowPosition
static void setWindowPosition(const vpImage< unsigned char > &I, int winx, int winy)
Definition: vpDisplay_uchar.cpp:1238
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:108
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:233
vpServo::set_eJe
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:406
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:242
vpRobot::setMaxTranslationVelocity
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
vpSphere::getR
double getR() const
Definition: vpSphere.h:84
vpWireFrameSimulator::getExternalImage
void getExternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:1074
vpWireFrameSimulator
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
Definition: vpWireFrameSimulator.h:154
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpWireFrameSimulator::set_fMo
void set_fMo(const vpHomogeneousMatrix &fMo_)
Definition: vpWireFrameSimulator.h:587
vpGenericFeature::get_s
void get_s(vpColVector &s) const
get the value of all the features.
Definition: vpGenericFeature.cpp:427
vpServo::set_cVe
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
vpGenericFeature
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
Definition: vpGenericFeature.h:182
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:119
vpWireFrameSimulator::setDesiredCameraPosition
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
Definition: vpWireFrameSimulator.h:514
vpDisplayD3D
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:106
vpSimulatorCamera
Class that defines the simplest robot: a free flying camera.
Definition: vpSimulatorCamera.h:107
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpWireFrameSimulator::setInternalCameraParameters
void setInternalCameraParameters(const vpCameraParameters &cam)
Definition: vpWireFrameSimulator.h:567
vpDisplay::displayFrame
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
Definition: vpDisplay_uchar.cpp:384
vpSphere::get_mu11
double get_mu11() const
Definition: vpSphere.h:77
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:141
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
vpWireFrameSimulator::initScene
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
Definition: vpWireFrameSimulator.cpp:261
vpParseArgv::parse
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:119
vpServo::print
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:740
vpDisplayGTK
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:137
vpWireFrameSimulator::setCameraPositionRelObj
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
Definition: vpWireFrameSimulator.h:457
vpRobot::setMaxRotationVelocity
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
vpSphere::get_x
double get_x() const
Definition: vpSphere.h:74
vpSphere::get_y
double get_y() const
Definition: vpSphere.h:75
vpSphere::get_mu02
double get_mu02() const
Definition: vpSphere.h:78
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:282
vpServo::DESIRED
@ DESIRED
Definition: vpServo.h:190
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:166
vpSphere::get_mu20
double get_mu20() const
Definition: vpSphere.h:76
vpSimulatorCamera::get_eJe
void get_eJe(vpMatrix &eJe)
Definition: vpSimulatorCamera.cpp:108
vpColor::none
static const vpColor none
Definition: vpColor.h:191
vpSphere
Class that defines what is a sphere.
Definition: vpSphere.h:60
vpWireFrameSimulator::setExternalCameraParameters
void setExternalCameraParameters(const vpCameraParameters &cam)
Definition: vpWireFrameSimulator.h:536
vpRobot::CAMERA_FRAME
@ CAMERA_FRAME
Definition: vpRobot.h:82
vpServo::EYEINHAND_L_cVe_eJe
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:163
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
vpGenericFeature::set_s
void set_s(const vpColVector &s)
set the value of all the features.
Definition: vpGenericFeature.cpp:405
vpServo
Definition: vpServo.h:150
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:716
vpRobotSimulator::setSamplingTime
virtual void setSamplingTime(const double &delta_t)
Definition: vpRobotSimulator.h:91
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:641
vpImage< vpRGBa >
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:765
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:149
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:198
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpForwardProjection::track
void track(const vpHomogeneousMatrix &cMo)
Definition: vpForwardProjection.cpp:111
vpWireFrameSimulator::getExternalCameraPosition
vpHomogeneousMatrix getExternalCameraPosition() const
Definition: vpWireFrameSimulator.h:350
vpWireFrameSimulator::getInternalImage
void getInternalImage(vpImage< unsigned char > &I)
Definition: vpWireFrameSimulator.cpp:1000