50 #include <visp/vpDebug.h>
51 #include <visp/vpViper850.h>
52 #include <visp/vpMath.h>
53 #include <visp/vpXmlParserCamera.h>
55 #ifdef VISP_HAVE_ACCESS_TO_NAS
56 static const char *opt_viper850[] = {
"CAMERA",
"eMc_ROT_XYZ",
"eMc_TRANS_XYZ",
61 =
"Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
63 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_without_distortion_Viper850.cnf";
68 =
"Z:/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
70 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_MarlinF033C_with_distortion_Viper850.cnf";
75 =
"Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
77 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_without_distortion_Viper850.cnf";
82 =
"Z:/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
84 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_PTGreyFlea2_with_distortion_Viper850.cnf";
89 =
"Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
91 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_without_distortion_Viper850.cnf";
96 =
"Z:/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
98 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_schunk_gripper_with_distortion_Viper850.cnf";
103 =
"Z:/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
105 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_without_distortion_Viper850.cnf";
110 =
"Z:/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
112 =
"/udd/fspindle/robot/Viper850/current/include/const_eMc_generic_with_distortion_Viper850.cnf";
118 =
"Z:/robot/Viper850/current/include/const_camera_Viper850.xml";
120 =
"/udd/fspindle/robot/Viper850/current/include/const_camera_Viper850.xml";
124 #endif // VISP_HAVE_ACCESS_TO_NAS
150 c56 = -341.33 / 9102.22;
192 #ifdef VISP_HAVE_ACCESS_TO_NAS
222 #ifdef VISP_HAVE_ACCESS_TO_NAS
224 char filename_eMc [FILENAME_MAX];
230 snprintf(filename_eMc, FILENAME_MAX,
"%s",
233 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
239 snprintf(filename_eMc, FILENAME_MAX,
"%s",
242 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
253 snprintf(filename_eMc, FILENAME_MAX,
"%s",
256 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
262 snprintf(filename_eMc, FILENAME_MAX,
"%s",
265 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
276 snprintf(filename_eMc, FILENAME_MAX,
"%s",
279 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
285 snprintf(filename_eMc, FILENAME_MAX,
"%s",
288 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
299 snprintf(filename_eMc, FILENAME_MAX,
"%s",
302 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
308 snprintf(filename_eMc, FILENAME_MAX,
"%s",
311 _snprintf(filename_eMc, FILENAME_MAX,
"%s",
332 this->
init (filename_eMc);
334 #else // VISP_HAVE_ACCESS_TO_NAS
396 #endif // VISP_HAVE_ACCESS_TO_NAS
413 #ifdef VISP_HAVE_ACCESS_TO_NAS
419 char Ligne[FILENAME_MAX];
425 bool get_rot_eMc =
false;
426 bool get_trans_eMc =
false;
429 if ((fdtask = fopen(filename,
"r" )) == NULL)
434 "Impossible to read the config file.");
437 while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
439 if (
'#' == Ligne[0]) {
continue; }
440 sscanf(Ligne,
"%s", namoption);
441 dim = strlen(namoption);
444 NULL != opt_viper850[code];
447 if (strncmp(opt_viper850[code], namoption, dim) == 0)
458 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
464 rot_eMc[0] *= M_PI / 180.0;
465 rot_eMc[1] *= M_PI / 180.0;
466 rot_eMc[2] *= M_PI / 180.0;
471 sscanf(Ligne,
"%s %lf %lf %lf", namoption,
475 get_trans_eMc =
true;
480 "ligne #%d.", filename, numLn);
487 if (get_rot_eMc && get_trans_eMc) {
488 for (
unsigned int i=0; i < 3; i ++) {
490 etc[i] = trans_eMc[i];
576 const unsigned int &image_width,
577 const unsigned int &image_height)
579 #if defined(VISP_HAVE_XML2) && defined (VISP_HAVE_ACCESS_TO_NAS)
583 std::cout <<
"Get camera parameters for camera \""
585 <<
"from the XML file: \""
587 if (parser.
parse(cam,
593 "Impossible to read the camera parameters.");
598 std::cout <<
"Get camera parameters for camera \""
600 <<
"from the XML file: \""
602 if (parser.
parse(cam,
608 "Impossible to read the camera parameters.");
613 std::cout <<
"Get camera parameters for camera \""
615 <<
"from the XML file: \""
617 if (parser.
parse(cam,
623 "Impossible to read the camera parameters.");
628 std::cout <<
"Get camera parameters for camera \""
630 <<
"from the XML file: \""
632 if (parser.
parse(cam,
638 "Impossible to read the camera parameters.");
653 "Impossible to read the camera parameters.");
662 if (image_width == 640 && image_height == 480) {
663 std::cout <<
"Get default camera parameters for camera \""
675 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
677 "Impossible to read the camera parameters.");
684 if (image_width == 640 && image_height == 480) {
685 std::cout <<
"Get default camera parameters for camera \""
697 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
699 "Impossible to read the camera parameters.");
705 if (image_width == 640 && image_height == 480) {
706 std::cout <<
"Get default camera parameters for camera \""
718 vpTRACE(
"Cannot get default intrinsic camera parameters for this image resolution");
720 "Impossible to read the camera parameters.");
727 "Impossible to read the camera parameters.");
vpToolType getToolType()
Get the current tool type.
static const char *const CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME
vpHomogeneousMatrix eMc
End effector to camera transformation.
static const char *const CONST_CAMERA_FILENAME
Error that can be emited by the vpRobot class and its derivates.
Perspective projection without distortion model.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
unsigned int getWidth() const
static const char *const CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
XML parser to load and save intrinsic camera parameters.
static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME
The vpRotationMatrix considers the particular case of a rotation matrix.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
double c56
Mechanical coupling between joint 5 and joint 6.
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
static const char *const CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME
static const char *const CONST_GENERIC_CAMERA_NAME
vpCameraParametersProjType
static const char *const CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME
vpToolType
List of possible tools that can be attached to the robot end-effector.
Generic class defining intrinsic camera parameters.
static const char *const CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME
static const char *const CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Perspective projection with distortion model.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
static double rad(double deg)
static const char *const CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME
int parse(vpCameraParameters &cam, const char *filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
static const char *const CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
unsigned int getHeight() const
void parseConfigFile(const char *filename)
vpCameraParameters::vpCameraParametersProjType projModel
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME