Visual Servoing Platform  version 3.3.0
servoSimu4Points.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 <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/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/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, std::string ipath, const char *badparam);
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display);
void usage(const char *name, std::string ipath, 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 \n\
position from the object.\n\
\n\
The visual features used to compute the pose of the camera and \n\
thus the control law are four points.\n\
\n\
This demonstration explains also how to move the object around a world\n\
reference frame. Here, the movement is a rotation around the x and y axis\n\
at a given distance from the world frame. In fact the object trajectory\n\
is on a sphere whose center is the origin of the world frame.\n\
\n\
SYNOPSIS\n\
%s [-d] [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set mire.pgm image input path.\n\
From this path read \"mire/mire.pgm\" image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment variable \n\
produces the same behaviour than using this option.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-h\n\
Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'i':
ipath = optarg_;
break;
case 'd':
display = false;
break;
case 'h':
usage(argv[0], ipath, NULL);
return false;
break;
default:
usage(argv[0], ipath, optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], ipath, NULL);
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)
{
try {
bool opt_display = true;
std::string opt_ipath;
std::string env_ipath;
std::string ipath;
std::string filename;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, 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, 1.0, 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.2, 0, 0, 0);
// Position of the camera in the world frame
wMc = wMo * cMo.inverse();
// The four point used as visual features
vpPoint point[4];
point[0].setWorldCoordinates(-0.1, -0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
point[2].setWorldCoordinates(0.1, 0.1, 0);
point[1].setWorldCoordinates(0.1, -0.1, 0);
// Projection of the points
for (int i = 0; i < 4; i++)
point[i].track(cMo);
// Set the current visual feature
for (int i = 0; i < 4; i++)
vpFeatureBuilder::create(p[i], point[i]);
// Projection of the points
for (int i = 0; i < 4; i++)
point[i].track(cdMo);
for (int i = 0; i < 4; i++)
vpFeatureBuilder::create(pd[i], point[i]);
vpHomogeneousMatrix cMe; // Identity
task.set_cVe(cVe);
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
for (int i = 0; i < 4; i++)
task.addFeature(p[i], pd[i]);
task.setLambda(10);
std::list<vpImageSimulator> list;
for (int i = 0; i < 4; i++)
X[i].resize(3);
X[0][0] = -0.2;
X[0][1] = -0.2;
X[0][2] = 0;
X[1][0] = 0.2;
X[1][1] = -0.2;
X[1][2] = 0;
X[2][0] = 0.2;
X[2][1] = 0.2;
X[2][2] = 0;
X[3][0] = -0.2;
X[3][1] = 0.2;
X[3][2] = 0;
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
// environment variable value
if (!env_ipath.empty())
ipath = env_ipath;
if (!opt_ipath.empty())
ipath = opt_ipath;
filename = vpIoTools::createFilePath(ipath, "mire/mire.pgm");
imsim.init(filename.c_str(), X);
list.push_back(imsim);
// 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
// Computes the position of a camera which is fixed in the object frame
vpHomogeneousMatrix camoMf(0, 0.0, 1.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 = 2500;
// 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)) {
};
}
robot.setPosition(wMc);
// 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;
for (int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
v = task.computeControlLaw();
// Compute the movement of the object around the world reference frame.
vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5 * iter), 0, 0);
vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5 * iter), 0);
// Move the object in the world frame
wMo = b * c * a;
sim.set_fMo(wMo); // Move the object in the simulator
// Compute the position of the external view which is fixed in the
// object frame
camoMf.buildFrom(0, 0.0, 1.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:151
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
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
vpWireFrameSimulator::get_fMo
vpHomogeneousMatrix get_fMo() const
Definition: vpWireFrameSimulator.h:425
vpIoTools::getViSPImagesDataPath
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1292
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:234
vpServo::set_eJe
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:406
vpWireFrameSimulator::PLATE
@ PLATE
Definition: vpWireFrameSimulator.h:165
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:242
vpRobot::setMaxTranslationVelocity
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
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:155
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:93
vpWireFrameSimulator::set_fMo
void set_fMo(const vpHomogeneousMatrix &fMo_)
Definition: vpWireFrameSimulator.h:587
vpImageSimulator::init
void init(const vpImage< unsigned char > &I, vpColVector *X)
Definition: vpImageSimulator.cpp:1081
vpServo::set_cVe
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
vpColor::none
static const vpColor none
Definition: vpColor.h:191
vpImageSimulator
Class which enables to project an image in the 3D space and get the view of a virtual camera.
Definition: vpImageSimulator.h:143
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:107
vpSimulatorCamera
Class that defines the simplest robot: a free flying camera.
Definition: vpSimulatorCamera.h:108
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:142
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:165
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()
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:138
vpWireFrameSimulator::setCameraPositionRelObj
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
Definition: vpWireFrameSimulator.h:457
vpPoint::setWorldCoordinates
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
vpRobot::setMaxRotationVelocity
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
vpServo::getError
vpColVector getError() const
Definition: vpServo.h:282
vpServo::DESIRED
@ DESIRED
Definition: vpServo.h:190
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:167
vpIoTools::createFilePath
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1537
vpSimulatorCamera::get_eJe
void get_eJe(vpMatrix &eJe)
Definition: vpSimulatorCamera.cpp:108
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
vpFeaturePoint
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Definition: vpFeaturePoint.h:182
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
vpServo
Definition: vpServo.h:151
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 >
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
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:150
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:72
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