49 #include <visp3/core/vpConfig.h> 50 #include <visp3/core/vpDebug.h> 53 #ifdef VISP_HAVE_COIN3D_AND_GUI 55 #include <visp3/core/vpImage.h> 56 #include <visp3/core/vpCameraParameters.h> 57 #include <visp3/core/vpTime.h> 58 #include <visp3/ar/vpSimulator.h> 59 #include <visp3/core/vpMath.h> 60 #include <visp3/core/vpHomogeneousMatrix.h> 61 #include <visp3/visual_features/vpFeaturePointPolar.h> 62 #include <visp3/vs/vpServo.h> 63 #include <visp3/robot/vpSimulatorCamera.h> 64 #include <visp3/visual_features/vpFeatureBuilder.h> 65 #include <visp3/io/vpParseArgv.h> 66 #include <visp3/core/vpIoTools.h> 68 #define GETOPTARGS "di:h" 80 void usage(
const char *name,
const char *badparam, std::string ipath)
83 Simulation Servo 4points.\n\ 86 %s [-i <input image path>] [-d] [-h]\n", name);
90 -i <input image path> %s\n\ 91 Set image input path.\n\ 92 From this path read \"ViSP-images/iv/4points.iv\"\n\ 94 Setting the VISP_INPUT_IMAGE_PATH environment\n\ 95 variable produces the same behaviour than using\n\ 99 Disable the image display. This can be useful \n\ 100 for automatic tests using crontab under Unix or \n\ 101 using the task manager under Windows.\n\ 104 Print the help.\n\n", ipath.c_str());
107 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
125 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
132 case 'i': ipath = optarg;
break;
133 case 'd': display =
false;
break;
134 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
137 usage(argv[0], optarg, ipath);
return false;
break;
141 if ((c == 1) || (c == -1)) {
143 usage(argv[0], NULL, ipath);
144 std::cerr <<
"ERROR: " << std::endl;
145 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
153 void *mainLoop (
void *_simu)
161 float sampling_time = 0.040f;
196 for (
int i = 0 ; i < 4 ; i++) {
203 for (
int i = 0 ; i < 4 ; i++)
206 std::cout <<
"s: \n";
207 for (
int i=0; i < 4; i ++) {
208 printf(
"[%d] rho %f theta %f Z %f\n",
209 i, p[i].get_rho(), p[i].get_theta(), p[i].get_Z());
228 for (
int i=0; i < 4; i ++) {
234 std::cout <<
"s*: \n";
235 for (
int i=0; i < 4; i ++) {
236 printf(
"[%d] rho %f theta %f Z %f\n",
237 i, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
257 for (
int i = 0 ; i < 4 ; i++)
268 unsigned int iter=0 ;
270 while(iter++ < 200) {
278 for (
int i = 0 ; i < 4 ; i++)
280 point[i].
track(cMo) ;
291 char name[FILENAME_MAX];
292 sprintf(name,
"/tmp/image.%04u.external.png",iter) ;
293 std::cout << name << std::endl ;
295 sprintf(name,
"/tmp/image.%04u.internal.png",iter) ;
306 std::cout <<
"cMo:\n" << cMo << std::endl;
308 std::cout <<
"final pose:\n" << pose.t() << std::endl;
316 int main(
int argc,
const char ** argv)
319 std::string env_ipath;
320 std::string opt_ipath;
322 std::string filename;
323 bool opt_display =
true;
329 if (! env_ipath.empty())
333 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
338 if (!opt_ipath.empty())
343 if (!opt_ipath.empty() && !env_ipath.empty()) {
344 if (ipath != env_ipath) {
345 std::cout << std::endl
346 <<
"WARNING: " << std::endl;
347 std::cout <<
" Since -i <visp image path=" << ipath <<
"> " 348 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
349 <<
" we skip the environment variable." << std::endl;
354 if (opt_ipath.empty() && env_ipath.empty()){
355 usage(argv[0], NULL, ipath);
356 std::cerr << std::endl
357 <<
"ERROR:" << std::endl;
358 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " 360 <<
" environment variable to specify the location of the " << std::endl
361 <<
" image path where test images are located." << std::endl << std::endl;
379 simu.
load(filename.c_str()) ;
390 std::cout <<
"Catch an exception: " << e << std::endl;
398 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void write(const char *fileName)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void setMaxTranslationVelocity(const double maxVt)
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
void set_eJe(const vpMatrix &eJe_)
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
error that can be emited by ViSP classes.
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix getPosition() const
virtual void mainLoop()
activate the mainloop
Class that defines 2D image point visual feature with polar coordinates described in ...
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
virtual void setSamplingTime(const double &delta_t)
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void load(const char *file_name)
load an iv file
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void initMainApplication()
perform some initialization in the main program thread
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Implementation of a pose vector and operations on poses.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
void get_eJe(vpMatrix &eJe)
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
void setZoomFactor(const float zoom)
set the size of the camera/frame