57#include <visp3/core/vpConfig.h>
58#include <visp3/core/vpDebug.h>
59#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
61#include <visp3/core/vpDisplay.h>
62#include <visp3/core/vpImage.h>
63#include <visp3/gui/vpDisplayGTK.h>
64#include <visp3/gui/vpDisplayOpenCV.h>
65#include <visp3/gui/vpDisplayX.h>
66#include <visp3/sensor/vp1394TwoGrabber.h>
68#include <visp3/blob/vpDot2.h>
69#include <visp3/core/vpException.h>
70#include <visp3/core/vpHomogeneousMatrix.h>
71#include <visp3/core/vpIoTools.h>
72#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
73#include <visp3/core/vpMath.h>
74#include <visp3/core/vpPoint.h>
75#include <visp3/io/vpParseArgv.h>
76#include <visp3/robot/vpRobotAfma4.h>
77#include <visp3/visual_features/vpFeatureBuilder.h>
78#include <visp3/visual_features/vpFeaturePoint.h>
79#include <visp3/vs/vpAdaptiveGain.h>
80#include <visp3/vs/vpServo.h>
81#include <visp3/vs/vpServoDisplay.h>
84#define GETOPTARGS "hK:l:"
86typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
97void usage(
const char *name,
const char *badparam, KalmanType &kalman)
100Tests a control law with the following characteristics:\n\
101- eye-in-hand control\n\
102- camera velocity are computed\n\
103- servo on 1 points.\n\
107 %s [-K <0|1|2|3>] [-h]\n", name);
112 Set the constant gain. By default adaptive gain. \n\
118 2: acceleration model\n\
121 Print the help.\n", (
int)kalman);
124 fprintf(stderr,
"ERROR: \n");
125 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
143bool getOptions(
int argc,
const char **argv, KalmanType &kalman,
bool &doAdaptativeGain,
152 kalman = (KalmanType)atoi(optarg);
155 doAdaptativeGain =
false;
159 usage(argv[0], NULL, kalman);
164 usage(argv[0], optarg, kalman);
170 if ((c == 1) || (c == -1)) {
172 usage(argv[0], NULL, kalman);
173 std::cerr <<
"ERROR: " << std::endl;
174 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
181int main(
int argc,
const char **argv)
184 KalmanType opt_kalman = K_NONE;
186 bool doAdaptativeGain =
true;
188 int opt_cam_frequency = 60;
191 if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) ==
false) {
201 std::string username;
206 std::string logdirname;
207 logdirname =
"/tmp/" + username;
215 std::cerr << std::endl <<
"ERROR:" << std::endl;
216 std::cerr <<
" Cannot create " << logdirname << std::endl;
220 std::string logfilename;
221 logfilename = logdirname +
"/log.dat";
224 std::ofstream flog(logfilename.c_str());
231 switch (opt_cam_frequency) {
244 for (
int i = 0; i < 10; i++)
248 vpDisplayX display(I, 100, 100,
"Current image");
249#elif defined(VISP_HAVE_OPENCV)
251#elif defined(VISP_HAVE_GTK)
258 std::cout << std::endl;
259 std::cout <<
"-------------------------------------------------------" << std::endl;
260 std::cout <<
"Test program for target motion compensation using a Kalman "
263 std::cout <<
"Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
264 std::cout <<
"Task : servo a point \n" << std::endl;
267 switch (opt_kalman) {
269 std::cout <<
"Servo with no target motion compensation (see -K option)\n";
272 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
273 <<
"with constant velocity modelization (see -K option)\n";
276 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
277 <<
"with constant acceleration modelization (see -K option)\n";
280 std::cout <<
"-------------------------------------------------------" << std::endl;
281 std::cout << std::endl;
285 std::cout <<
"Click on the dot..." << std::endl;
316 std::cout << std::endl;
333 unsigned int nsignal = 2;
337 unsigned int state_size = 0;
339 switch (opt_kalman) {
344 sigma_state.
resize(state_size * nsignal);
345 sigma_state = 0.00001;
346 sigma_measure = 0.05;
348 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
352 case K_ACCELERATION: {
356 sigma_state.
resize(state_size * nsignal);
357 sigma_state = 0.00001;
358 sigma_measure = 0.05;
359 double dt = 1. / opt_cam_frequency;
360 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
388 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
392 double Tv = (double)(t_0 - t_1) / 1000.0;
441 dedt_mes = (err_0 - err_1) / (Tv_1)-task.
J1 * vm_0;
452 switch (opt_kalman) {
459 for (
unsigned int i = 0; i < nsignal; i++) {
460 dedt_filt[i] = kalman.
Xest[i * state_size];
467 v2 = -J1p * dedt_filt;
490 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
495 flog << task.
error[0] <<
" " << task.
error[1] <<
" ";
498 flog << cog.
get_u() - cam.get_u0() <<
" " << cog.
get_v() - cam.get_v0() <<
" ";
501 flog << dedt_mes[0] <<
" " << dedt_mes[1] <<
" ";
504 flog << dedt_filt[0] <<
" " << dedt_filt[1] <<
" ";
521 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
529 std::cout <<
"You do not have an afma4 robot connected to your computer..." << std::endl;
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
Adaptive gain computation.
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
void initFromConstant(double c)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static const vpColor blue
static const vpColor green
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...
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
void setGraphics(bool activate)
vpImagePoint getCog() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
error that can be emited by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
unsigned int getHeight() const
unsigned int getStateSize()
This class provides an implementation of some specific linear Kalman filters.
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filter(vpColVector &z)
void setStateModel(vpStateModel model)
@ stateConstVelWithColoredNoise_MeasureVel
@ stateConstAccWithColoredNoise_MeasureVel
Implementation of a matrix and operations on matrices.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Control of Irisa's cylindrical robot named Afma4.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
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)
vpMatrix J1
Task Jacobian .
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
vpMatrix getTaskJacobianPseudoInverse() const
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
VISP_EXPORT double measureTimeMs()