53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h>
55 #if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
57 #include <visp3/core/vpDisplay.h>
58 #include <visp3/core/vpImage.h>
59 #include <visp3/gui/vpDisplayGTK.h>
60 #include <visp3/gui/vpDisplayOpenCV.h>
61 #include <visp3/gui/vpDisplayX.h>
62 #include <visp3/sensor/vp1394TwoGrabber.h>
64 #include <visp3/blob/vpDot2.h>
65 #include <visp3/core/vpException.h>
66 #include <visp3/core/vpHomogeneousMatrix.h>
67 #include <visp3/core/vpIoTools.h>
68 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
69 #include <visp3/core/vpMath.h>
70 #include <visp3/core/vpPoint.h>
71 #include <visp3/io/vpParseArgv.h>
72 #include <visp3/robot/vpRobotAfma4.h>
73 #include <visp3/visual_features/vpFeatureBuilder.h>
74 #include <visp3/visual_features/vpFeaturePoint.h>
75 #include <visp3/vs/vpAdaptiveGain.h>
76 #include <visp3/vs/vpServo.h>
77 #include <visp3/vs/vpServoDisplay.h>
80 #define GETOPTARGS "hK:l:"
82 typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
93 void usage(
const char *name,
const char *badparam, KalmanType &kalman)
96 Tests a control law with the following characteristics:\n\
97 - eye-in-hand control\n\
98 - camera velocity are computed\n\
99 - servo on 1 points.\n\
100 - Kalman filtering\n\
103 %s [-K <0|1|2|3>] [-h]\n",
109 Set the constant gain. By default adaptive gain. \n\
115 2: acceleration model\n\
122 fprintf(stderr,
"ERROR: \n");
123 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
141 bool getOptions(
int argc,
const char **argv, KalmanType &kalman,
bool &doAdaptativeGain,
150 kalman = (KalmanType)atoi(optarg);
153 doAdaptativeGain =
false;
157 usage(argv[0],
nullptr, kalman);
162 usage(argv[0], optarg, kalman);
168 if ((c == 1) || (c == -1)) {
170 usage(argv[0],
nullptr, kalman);
171 std::cerr <<
"ERROR: " << std::endl;
172 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
179 int main(
int argc,
const char **argv)
182 KalmanType opt_kalman = K_NONE;
184 bool doAdaptativeGain =
true;
186 int opt_cam_frequency = 60;
189 if (getOptions(argc, argv, opt_kalman, doAdaptativeGain, lambda) ==
false) {
199 std::string username;
204 std::string logdirname;
205 logdirname =
"/tmp/" + username;
213 std::cerr << std::endl <<
"ERROR:" << std::endl;
214 std::cerr <<
" Cannot create " << logdirname << std::endl;
218 std::string logfilename;
219 logfilename = logdirname +
"/log.dat";
222 std::ofstream flog(logfilename.c_str());
229 switch (opt_cam_frequency) {
242 for (
int i = 0; i < 10; i++)
247 #elif defined(HAVE_OPENCV_HIGHGUI)
249 #elif defined(VISP_HAVE_GTK)
256 std::cout << std::endl;
257 std::cout <<
"-------------------------------------------------------" << std::endl;
258 std::cout <<
"Test program for target motion compensation using a Kalman "
261 std::cout <<
"Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
262 std::cout <<
"Task : servo a point \n" << std::endl;
265 switch (opt_kalman) {
267 std::cout <<
"Servo with no target motion compensation (see -K option)\n";
270 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
271 <<
"with constant velocity modelization (see -K option)\n";
274 std::cout <<
"Servo with target motion compensation using a Kalman filter\n"
275 <<
"with constant acceleration modelization (see -K option)\n";
278 std::cout <<
"-------------------------------------------------------" << std::endl;
279 std::cout << std::endl;
283 std::cout <<
"Click on the dot..." << std::endl;
314 std::cout << std::endl;
331 unsigned int nsignal = 2;
335 unsigned int state_size = 0;
337 switch (opt_kalman) {
342 sigma_state.
resize(state_size * nsignal);
343 sigma_state = 0.00001;
344 sigma_measure = 0.05;
346 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
350 case K_ACCELERATION: {
354 sigma_state.
resize(state_size * nsignal);
355 sigma_state = 0.00001;
356 sigma_measure = 0.05;
357 double dt = 1. / opt_cam_frequency;
358 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
386 std::cout <<
"\nHit CTRL-C to stop the loop...\n" << std::flush;
390 double Tv = (double)(t_0 - t_1) / 1000.0;
439 dedt_mes = (err_0 - err_1) / (Tv_1)-task.
J1 * vm_0;
450 switch (opt_kalman) {
457 for (
unsigned int i = 0; i < nsignal; i++) {
458 dedt_filt[i] = kalman.
Xest[i * state_size];
465 v2 = -J1p * dedt_filt;
488 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
493 flog << task.
error[0] <<
" " << task.
error[1] <<
" ";
496 flog << cog.
get_u() - cam.get_u0() <<
" " << cog.
get_v() - cam.get_v0() <<
" ";
499 flog << dedt_mes[0] <<
" " << dedt_mes[1] <<
" ";
502 flog << dedt_filt[0] <<
" " << dedt_filt[1] <<
" ";
519 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
527 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 emitted 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 getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ 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 addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
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 display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT double measureTimeMs()