Visual Servoing Platform  version 3.3.0
manSimu4Dots.cpp

Visual servoing experiment on 4 points with a visualization and image generation from the camera and from an external view using vpSimulator.

/****************************************************************************
*
* 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:
* Simulation of a visual servoing with visualization and image generation.
*
* Authors:
* Eric Marchand
* Fabien Spindler
*
*****************************************************************************/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
#include <visp3/ar/vpSimulator.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpTime.h>
#if defined(VISP_HAVE_X11)
#include <visp3/gui/vpDisplayX.h>
#elif defined(VISP_HAVE_GDI)
#include <visp3/gui/vpDisplayGDI.h>
#elif defined(VISP_HAVE_GTK)
#include <visp3/gui/vpDisplayGTK.h>
#endif
// You may have strange compiler issues using the simulator based on SoQt
// and the vpDisplayGTK. In that case prefer to use another display like
// vpDisplayX under linux or vpDisplayGDI under Windows
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/vision/vpPose.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
static void *mainLoop(void *_simu)
{
// pointer copy of the vpSimulator instance
vpSimulator *simu = static_cast<vpSimulator *> (_simu);
// Simulation initialization
// Set the initial camera location
vpHomogeneousMatrix cMo(0.3, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(40));
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
// Initialize the robot
robot.setSamplingTime(0.04); // 40ms
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
// Send the robot position to the visualizator
simu->setCameraPosition(cMo);
// Initialize the camera parameters
simu->getCameraParameters(cam);
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
vpPoint point[4];
point[0].setWorldCoordinates(-0.1, -0.1, 0);
point[1].setWorldCoordinates(0.1, -0.1, 0);
point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
// sets the desired camera location
vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
// computes the 3D point coordinates in the camera frame and its 2D
// coordinates
for (int i = 0; i < 4; i++)
point[i].project(cMo_d);
// creates the associated features
for (int i = 0; i < 4; i++)
vpFeatureBuilder::create(pd[i], point[i]);
// Current visual features initialization
unsigned int height = simu->getInternalHeight();
unsigned int width = simu->getInternalWidth();
// Create a greyscale image
vpImage<unsigned char> I(height, width);
// Display initialization
#if defined(VISP_HAVE_X11)
vpDisplayX disp;
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_GTK)
#endif
disp.init(I, 100, 100, "Simulation display");
// disp(I);
// Get the current image
vpTime::wait(500); // wait to be sure the image is generated
simu->getInternalImage(I);
// Display the current image
// Initialize the four dots tracker
std::cout << "A click in the four dots clockwise. " << std::endl;
vpDot2 dot[4];
for (int i = 0; i < 4; i++) {
dot[i].setGraphics(true);
// Call for a click
std::cout << "A click in the dot " << i << std::endl;
dot[i].initTracking(I);
// Create the associated feature
vpFeatureBuilder::create(p[i], cam, dot[i]);
// flush the display
}
// Task defintion
vpServo task;
// we want an eye-in-hand control law ;
// Set the position of the end-effector frame in the camera frame as identity
task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
// we want to see a point on a point
for (int i = 0; i < 4; i++)
task.addFeature(p[i], pd[i]);
// Set the gain
task.setLambda(1.0);
// Print the current information about the task
task.print();
// The control loop
int k = 0;
while (k++ < 200) {
double t = vpTime::measureTimeMs();
// Get the current internal camera view and display it
simu->getInternalImage(I);
// Track the four dots and update the associated visual features
for (int i = 0; i < 4; i++) {
dot[i].track(I);
vpFeatureBuilder::create(p[i], cam, dot[i]);
}
// Display the desired and current visual features
vpServoDisplay::display(task, cam, I);
// Update the robot Jacobian
robot.get_eJe(eJe);
task.set_eJe(eJe);
// Compute the control law
// Send the computed velocity to the robot and compute the new robot
// position
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
// Send the robot position to the visualizator
simu->setCameraPosition(cMo);
// Wait 40 ms
vpTime::wait(t, 40);
}
// Print information about the task
task.print();
task.kill();
void *a = NULL;
return a;
}
int main()
{
try {
// Internal view initialization : view from the robot camera
simu.initInternalViewer(480, 360);
// External view initialization : view from an external camera
simu.initExternalViewer(300, 300);
// Inernal camera paramters initialization
vpCameraParameters cam(800, 800, 240, 180);
// Load the scene
// Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
// environment variable value
std::string ipath = vpIoTools::getViSPImagesDataPath();
std::string filename = "./4points.iv";
// Set the default input path
if (!ipath.empty())
filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
std::cout << "Load : " << filename << std::endl << "This file should be in the working directory" << std::endl;
simu.load(filename.c_str());
// Run the main loop
simu.initApplication(&mainLoop);
// Run the simulator
simu.mainLoop();
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, GTK, or OpenCV, or GDI (Graphical Device Interface) 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
vpRobot::ARTICULAR_FRAME
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
vpSimulator::mainLoop
virtual void mainLoop()
activate the mainloop
Definition: vpSimulator.cpp:642
vpServo::kill
void kill()
Definition: vpServo.cpp:192
vpSimulator::initExternalViewer
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
Definition: vpSimulator.cpp:484
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
vpIoTools::getViSPImagesDataPath
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1292
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
vpSimulator::getInternalImage
void getInternalImage(vpImage< unsigned char > &I)
get an Image of the internal view
Definition: vpSimulator.cpp:990
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:242
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpDot2
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:126
vpSimulator::initApplication
void initApplication(void *(*start_routine)(void *))
begin the main program
Definition: vpSimulator.cpp:823
vpFeatureBuilder::create
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Definition: vpFeatureBuilderPoint.cpp:93
vpSimulator::load
void load(const char *file_name)
load an iv file
Definition: vpSimulator.cpp:662
vpServo::set_cVe
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
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
vpSimulator::getInternalHeight
unsigned int getInternalHeight() const
Definition: vpSimulator.h:180
vpSimulator::initMainApplication
void initMainApplication()
perform some initialization in the main program thread
Definition: vpSimulator.cpp:845
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
vpSimulator::getCameraParameters
void getCameraParameters(vpCameraParameters &cam)
get the intrinsic parameters of the camera
Definition: vpSimulator.h:293
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
vpPoint::setWorldCoordinates
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
vpSimulator::getInternalWidth
unsigned int getInternalWidth() const
Definition: vpSimulator.h:174
vpServo::DESIRED
@ DESIRED
Definition: vpServo.h:190
vpDot2::initTracking
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
Definition: vpDot2.cpp:253
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:166
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
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
vpSimulator::setInternalCameraParameters
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Definition: vpSimulator.cpp:511
vpFeaturePoint
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Definition: vpFeaturePoint.h:181
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
vpServo
Definition: vpServo.h:150
vpSimulator::initInternalViewer
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
Definition: vpSimulator.cpp:450
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
vpDisplayGTK::init
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Definition: vpDisplayGTK.cpp:258
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
vpImage< unsigned char >
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:641
vpDot2::track
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
Definition: vpDot2.cpp:441
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:58
vpSimulator::setCameraPosition
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Definition: vpSimulator.cpp:580
vpSimulator
Implementation of a simulator based on Coin3d (www.coin3d.org).
Definition: vpSimulator.h:99
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
vpServoDisplay::display
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
Definition: vpServoDisplay.cpp:80
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpSimulator::closeMainApplication
void closeMainApplication()
Definition: vpSimulator.cpp:853
vpDot2::setGraphics
void setGraphics(bool activate)
Definition: vpDot2.h:294