59 #include <visp3/core/vpConfig.h> 60 #include <visp3/core/vpDebug.h> 62 #if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394)) 64 #include <visp3/sensor/vp1394TwoGrabber.h> 65 #include <visp3/core/vpImage.h> 66 #include <visp3/core/vpDisplay.h> 67 #include <visp3/gui/vpDisplayX.h> 68 #include <visp3/gui/vpDisplayOpenCV.h> 69 #include <visp3/gui/vpDisplayGTK.h> 71 #include <visp3/core/vpMath.h> 72 #include <visp3/core/vpHomogeneousMatrix.h> 73 #include <visp3/visual_features/vpFeaturePoint.h> 74 #include <visp3/core/vpPoint.h> 75 #include <visp3/vs/vpServo.h> 76 #include <visp3/visual_features/vpFeatureBuilder.h> 77 #include <visp3/robot/vpRobotAfma4.h> 78 #include <visp3/core/vpIoTools.h> 79 #include <visp3/core/vpException.h> 80 #include <visp3/vs/vpServoDisplay.h> 81 #include <visp3/io/vpParseArgv.h> 82 #include <visp3/blob/vpDot2.h> 83 #include <visp3/vs/vpAdaptiveGain.h> 84 #include <visp3/core/vpLinearKalmanFilterInstantiation.h> 88 #define GETOPTARGS "hK:l:" 105 void usage(
const char *name,
const char *badparam,
109 Tests a control law with the following characteristics:\n\ 110 - eye-in-hand control\n\ 111 - camera velocity are computed\n\ 112 - servo on 1 points.\n\ 113 - Kalman filtering\n\ 116 %s [-K <0|1|2|3>] [-h]\n", name);
121 Set the constant gain. By default adaptive gain. \n\ 127 2: acceleration model\n\ 130 Print the help.\n", (
int) kalman);
133 fprintf(stderr,
"ERROR: \n" );
134 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
153 bool getOptions(
int argc,
const char **argv, KalmanType &kalman,
154 bool &doAdaptativeGain,
163 kalman = (KalmanType) atoi(optarg);
166 doAdaptativeGain =
false;
169 case 'h': usage(argv[0], NULL, kalman);
173 usage(argv[0], optarg, kalman);
178 if ((c == 1) || (c == -1)) {
180 usage(argv[0], NULL, kalman);
181 std::cerr <<
"ERROR: " << std::endl;
182 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
190 main(
int argc,
const char ** argv)
193 KalmanType opt_kalman = K_NONE;
195 bool doAdaptativeGain =
true;
197 int opt_cam_frequency = 60;
200 if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) ==
false) {
210 std::string username;
215 std::string logdirname;
216 logdirname =
"/tmp/" + username;
225 std::cerr << std::endl
226 <<
"ERROR:" << std::endl;
227 std::cerr <<
" Cannot create " << logdirname << std::endl;
231 std::string logfilename;
232 logfilename = logdirname +
"/log.dat";
235 std::ofstream flog(logfilename.c_str());
242 switch(opt_cam_frequency) {
249 for (
int i=0; i < 10; i++)
253 vpDisplayX display(I,100,100,
"Current image") ;
254 #elif defined(VISP_HAVE_OPENCV) 256 #elif defined(VISP_HAVE_GTK) 263 std::cout << std::endl ;
264 std::cout <<
"-------------------------------------------------------" << std::endl ;
265 std::cout <<
"Test program for target motion compensation using a Kalman filter " <<std::endl ;
266 std::cout <<
"Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
267 std::cout <<
"Task : servo a point \n" << std::endl ;
272 std::cout <<
"Servo with no target motion compensation (see -K option)\n";
275 std::cout <<
"Servo with target motion compensation using a Kalman filter\n" 276 <<
"with constant velocity modelization (see -K option)\n";
279 std::cout <<
"Servo with target motion compensation using a Kalman filter\n" 280 <<
"with constant acceleration modelization (see -K option)\n";
283 std::cout <<
"-------------------------------------------------------" << std::endl ;
284 std::cout << std::endl ;
288 std::cout <<
"Click on the dot..." << std::endl;
319 std::cout << std::endl ;
320 task.addFeature(p,pd) ;
323 task.setLambda(lambda) ;
336 unsigned int nsignal = 2;
340 unsigned int state_size = 0;
347 sigma_state.
resize(state_size*nsignal);
348 sigma_state = 0.00001;
349 sigma_measure = 0.05;
351 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
355 case K_ACCELERATION: {
359 sigma_state.
resize(state_size*nsignal);
360 sigma_state = 0.00001;
361 sigma_measure = 0.05;
362 double dt = 1./opt_cam_frequency;
363 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt );
392 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
396 double Tv = (double)(t_0 - t_1) / 1000.0;
430 v1 = task.computeControlLaw() ;
445 dedt_mes = (err_0 - err_1)/(Tv_1) - task.J1*vm_0;
463 for (
unsigned int i=0; i < nsignal; i++) {
464 dedt_filt[i] = kalman.
Xest[i*state_size];
470 vpMatrix J1p = task.getTaskJacobianPseudoInverse();
471 v2 = - J1p*dedt_filt;
495 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " 496 << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
501 flog << task.error[0] <<
" " << task.error[1] <<
" ";
508 flog << dedt_mes[0] <<
" " << dedt_mes[1] <<
" ";
511 flog << dedt_filt[0] <<
" " << dedt_filt[1] <<
" ";
533 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
544 vpERROR_TRACE(
"You do not have an afma4 robot or a firewire framegrabber connected to your computer...");
Implementation of a matrix and operations on matrices.
Adaptive gain computation.
unsigned int getStateSize()
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filter(vpColVector &z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
error that can be emited by ViSP classes.
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static const vpColor green
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void track(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Initialize the velocity controller.
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void initFromConstant(double c)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void buildFrom(const double x, const double y, const double Z)
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
unsigned int getHeight() const
Implementation of column vector and the associated operations.
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Control of Irisa's cylindrical robot named Afma4.
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
This class provides an implementation of some specific linear Kalman filters.
vpImagePoint getCog() const
void setStateModel(vpStateModel model)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
unsigned int getWidth() const
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)
void setGraphics(const bool activate)
static const vpColor blue
void resize(const unsigned int i, const bool flagNullify=true)