1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the ADEPT Viper 650 robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39 /*!
40
41 \file vpViper650.cpp
42
43 Modelisation of the ADEPT Viper 650 robot.
44
45 */
46
47 #include <visp3/core/vpDebug.h>
48 #include <visp3/core/vpMath.h>
49 #include <visp3/core/vpXmlParserCamera.h>
50 #include <visp3/robot/vpViper650.h>
51
52 static const char *opt_viper650[] = {"CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", NULL};
53
54 #ifdef VISP_HAVE_VIPER650_DATA
55 const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME =
56 std::string(VISP_VIPER650_DATA_PATH) +
57 std::string("/include/const_eMc_MarlinF033C_without_distortion_Viper650.cnf");
58
59 const std::string vpViper650::CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME =
60 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_MarlinF033C_with_distortion_Viper650.cnf");
61
62 const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME =
63 std::string(VISP_VIPER650_DATA_PATH) +
64 std::string("/include/const_eMc_PTGreyFlea2_without_distortion_Viper650.cnf");
65
66 const std::string vpViper650::CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME =
67 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_PTGreyFlea2_with_distortion_Viper650.cnf");
68
69 const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME =
70 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/"
71 "const_eMc_schunk_gripper_without_distortion_Viper650."
72 "cnf");
73
74 const std::string vpViper650::CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME =
75 std::string(VISP_VIPER650_DATA_PATH) +
76 std::string("/include/const_eMc_schunk_gripper_with_distortion_Viper650.cnf");
77
78 const std::string vpViper650::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME =
79 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Viper650.cnf");
80
81 const std::string vpViper650::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME =
82 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Viper650.cnf");
83
84 const std::string vpViper650::CONST_CAMERA_FILENAME =
85 std::string(VISP_VIPER650_DATA_PATH) + std::string("/include/const_camera_Viper650.xml");
86
87 #endif // VISP_HAVE_VIPER650_DATA
88
89 const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
90 const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
91 const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
92 const char *const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
93
94 const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA;
95
96 /*!
97
98 Default constructor.
99 Sets the specific parameters like the Denavit Hartenberg parameters.
100
101 */
vpViper650()102 vpViper650::vpViper650()
103 : tool_current(vpViper650::defaultTool), projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
104 {
105 // Denavit Hartenberg parameters
106 a1 = 0.075;
107 a2 = 0.270;
108 a3 = 0.090;
109 d1 = 0.335;
110 d4 = 0.295;
111 d6 = 0.080 + 0.1016; // To take into account the offset to go to the tool changer
112 c56 = -341.33 / 9102.22;
113
114 // Software joint limits in radians
115 joint_min[0] = vpMath::rad(-170);
116 joint_min[1] = vpMath::rad(-190);
117 joint_min[2] = vpMath::rad(-29);
118 joint_min[3] = vpMath::rad(-190);
119 joint_min[4] = vpMath::rad(-120);
120 joint_min[5] = vpMath::rad(-360);
121 joint_max[0] = vpMath::rad(170);
122 joint_max[1] = vpMath::rad(45);
123 joint_max[2] = vpMath::rad(256);
124 joint_max[3] = vpMath::rad(190);
125 joint_max[4] = vpMath::rad(120);
126 joint_max[5] = vpMath::rad(360);
127
128 init(); // Set the default tool
129 }
130
131 /*!
132
133 Initialize the robot with the default tool vpViper650::defaultTool.
134 */
init(void)135 void vpViper650::init(void)
136 {
137 this->init(vpViper650::defaultTool);
138 return;
139 }
140
141 /*!
142
143 Read files containing the constant parameters related to the robot
144 tools in order to set the end-effector to tool transformation.
145
146 \param camera_extrinsic_parameters : Filename containing the camera
147 extrinsic parameters.
148
149 */
init(const std::string & camera_extrinsic_parameters)150 void vpViper650::init(const std::string &camera_extrinsic_parameters)
151 {
152 // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
153 this->parseConfigFile(camera_extrinsic_parameters);
154
155 return;
156 }
157
158 /*!
159
160 Set the constant parameters related to the robot kinematics and to
161 the end-effector to camera transformation (\f$^e{\bf M}c\f$)
162 corresponding to the camera extrinsic parameters. These last
163 parameters depend on the camera and projection model in use and are
164 loaded from predefined files or parameters.
165
166 \warning If the macro VISP_HAVE_VIPER650_DATA is defined in vpConfig.h
167 this function reads the camera extrinsic parameters from the file
168 corresponding to the specified camera type and projection type.
169 Otherwise corresponding default parameters are loaded.
170
171 \param tool : Camera in use.
172
173 \param proj_model : Projection model of the camera.
174
175 \sa init(vpViper650::vpToolType, const std::string&),
176 init(vpViper650::vpToolType, const vpHomogeneousMatrix&)
177
178 */
init(vpViper650::vpToolType tool,vpCameraParameters::vpCameraParametersProjType proj_model)179 void vpViper650::init(vpViper650::vpToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model)
180 {
181
182 this->projModel = proj_model;
183
184 #ifdef VISP_HAVE_VIPER650_DATA
185 // Read the robot parameters from files
186 std::string filename_eMc;
187 switch (tool) {
188 case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
189 switch (projModel) {
190 case vpCameraParameters::perspectiveProjWithoutDistortion:
191 filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
192 break;
193 case vpCameraParameters::perspectiveProjWithDistortion:
194 filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
195 break;
196 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
197 throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
198 break;
199 }
200 break;
201 }
202 case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
203 switch (projModel) {
204 case vpCameraParameters::perspectiveProjWithoutDistortion:
205 filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
206 break;
207 case vpCameraParameters::perspectiveProjWithDistortion:
208 filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
209 break;
210 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
211 throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
212 break;
213 }
214 break;
215 }
216 case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
217 switch (projModel) {
218 case vpCameraParameters::perspectiveProjWithoutDistortion:
219 filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
220 break;
221 case vpCameraParameters::perspectiveProjWithDistortion:
222 filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
223 break;
224 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
225 throw vpException(vpException::notImplementedError, "Feature TOOL_SCHUNK_GRIPPER_CAMERA is not implemented for Kannala-Brandt projection model yet.");
226 break;
227 }
228 break;
229 }
230 case vpViper650::TOOL_GENERIC_CAMERA: {
231 switch (projModel) {
232 case vpCameraParameters::perspectiveProjWithoutDistortion:
233 filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
234 break;
235 case vpCameraParameters::perspectiveProjWithDistortion:
236 filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
237 break;
238 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
239 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
240 break;
241 }
242 break;
243 }
244 case vpViper650::TOOL_CUSTOM: {
245 throw vpRobotException(vpRobotException::badValue,
246 "No predefined file available for a custom tool"
247 "You should use init(vpViper650::vpToolType, const std::string&) or"
248 "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
249 }
250 default: {
251 vpERROR_TRACE("This error should not occur!");
252 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
253 // "que les specs de la classe ont ete modifiee, "
254 // "et que le code n'a pas ete mis a jour "
255 // "correctement.");
256 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
257 // "vpViper650::vpViper650ToolType, et controlez que "
258 // "tous les cas ont ete pris en compte dans la "
259 // "fonction init(camera).");
260 break;
261 }
262 }
263
264 this->init(filename_eMc);
265
266 #else // VISP_HAVE_VIPER650_DATA
267
268 // Use here default values of the robot constant parameters.
269 switch (tool) {
270 case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
271 switch (projModel) {
272 case vpCameraParameters::perspectiveProjWithoutDistortion:
273 erc[0] = vpMath::rad(0.07); // rx
274 erc[1] = vpMath::rad(2.76); // ry
275 erc[2] = vpMath::rad(-91.50); // rz
276 etc[0] = -0.0453; // tx
277 etc[1] = 0.0005; // ty
278 etc[2] = 0.0728; // tz
279 break;
280 case vpCameraParameters::perspectiveProjWithDistortion:
281 erc[0] = vpMath::rad(0.26); // rx
282 erc[1] = vpMath::rad(2.12); // ry
283 erc[2] = vpMath::rad(-91.31); // rz
284 etc[0] = -0.0444; // tx
285 etc[1] = -0.0005; // ty
286 etc[2] = 0.1022; // tz
287 break;
288 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
289 throw vpException(vpException::notImplementedError, "Feature TOOL_MARLIN_F033C_CAMERA is not implemented for Kannala-Brandt projection model yet.");
290 break;
291 }
292 break;
293 }
294 case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
295 case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
296 switch (projModel) {
297 case vpCameraParameters::perspectiveProjWithoutDistortion:
298 erc[0] = vpMath::rad(0.15); // rx
299 erc[1] = vpMath::rad(1.28); // ry
300 erc[2] = vpMath::rad(-90.8); // rz
301 etc[0] = -0.0456; // tx
302 etc[1] = -0.0013; // ty
303 etc[2] = 0.001; // tz
304 break;
305 case vpCameraParameters::perspectiveProjWithDistortion:
306 erc[0] = vpMath::rad(0.72); // rx
307 erc[1] = vpMath::rad(2.10); // ry
308 erc[2] = vpMath::rad(-90.5); // rz
309 etc[0] = -0.0444; // tx
310 etc[1] = -0.0012; // ty
311 etc[2] = 0.078; // tz
312 break;
313 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
314 throw vpException(vpException::notImplementedError, "Feature TOOL_PTGREY_FLEA2_CAMERA is not implemented for Kannala-Brandt projection model yet.");
315 break;
316 }
317 break;
318 }
319 case vpViper650::TOOL_GENERIC_CAMERA: {
320 // Set eMc to identity
321 switch (projModel) {
322 case vpCameraParameters::perspectiveProjWithoutDistortion:
323 case vpCameraParameters::perspectiveProjWithDistortion:
324 erc[0] = 0; // rx
325 erc[1] = 0; // ry
326 erc[2] = 0; // rz
327 etc[0] = 0; // tx
328 etc[1] = 0; // ty
329 etc[2] = 0; // tz
330 break;
331 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
332 throw vpException(vpException::notImplementedError, "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
333 break;
334 }
335 break;
336 }
337 case vpViper650::TOOL_CUSTOM: {
338 throw vpRobotException(vpRobotException::badValue,
339 "No predefined parameters available for a custom tool"
340 "You should use init(vpViper650::vpToolType, const std::string&) or"
341 "init(vpViper650::vpToolType, const vpHomogeneousMatrix&) instead");
342 }
343 }
344 vpRotationMatrix eRc(erc);
345 this->eMc.buildFrom(etc, eRc);
346 #endif // VISP_HAVE_VIPER650_DATA
347
348 setToolType(tool);
349 return;
350 }
351
352 /*!
353
354 Set the type of tool attached to the robot and transformation
355 between the end-effector and the tool (\f$^e{\bf M}c\f$).
356 This last parameter is loaded from a file.
357
358 \param tool : Type of tool in use.
359
360 \param filename : Path of the configuration file containing the
361 transformation between the end-effector frame and the tool frame.
362
363 The configuration file should have the form below:
364
365 \code
366 # Start with any number of consecutive lines
367 # beginning with the symbol '#'
368 #
369 # The 3 following lines contain the name of the camera,
370 # the rotation parameters of the geometric transformation
371 # using the Euler angles in degrees with convention XYZ and
372 # the translation parameters expressed in meters
373 CAMERA CameraName
374 eMc_ROT_XYZ 10.0 -90.0 20.0
375 eMc_TRANS_XYZ 0.05 0.01 0.06
376 \endcode
377
378 \sa init(vpViper650::vpToolType,
379 vpCameraParameters::vpCameraParametersProjType), init(vpViper650::vpToolType,
380 const vpHomogeneousMatrix&)
381 */
init(vpViper650::vpToolType tool,const std::string & filename)382 void vpViper650::init(vpViper650::vpToolType tool, const std::string &filename)
383 {
384 this->setToolType(tool);
385 this->parseConfigFile(filename.c_str());
386 }
387
388 /*!
389
390 Set the type of tool attached to the robot and the transformation
391 between the end-effector and the tool (\f$^e{\bf M}c\f$).
392
393 \param tool : Type of tool in use.
394
395 \param eMc_ : Homogeneous matrix representation of the transformation
396 between the end-effector frame and the tool frame.
397
398 \sa init(vpViper650::vpToolType,
399 vpCameraParameters::vpCameraParametersProjType),
400 init(vpViper650::vpToolType, const std::string&)
401
402 */
init(vpViper650::vpToolType tool,const vpHomogeneousMatrix & eMc_)403 void vpViper650::init(vpViper650::vpToolType tool, const vpHomogeneousMatrix &eMc_)
404 {
405 this->setToolType(tool);
406 this->set_eMc(eMc_);
407 }
408
409 /*!
410
411 This function gets the robot constant parameters from a file.
412
413 \param filename : File name containing the robot constant
414 parameters, like the hand-to-eye transformation.
415
416 */
parseConfigFile(const std::string & filename)417 void vpViper650::parseConfigFile(const std::string &filename)
418 {
419 vpRxyzVector erc_; // eMc rotation
420 vpTranslationVector etc_; // eMc translation
421
422 std::ifstream fdconfig(filename.c_str(), std::ios::in);
423
424 if (!fdconfig.is_open()) {
425 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
426 filename.c_str());
427 }
428
429 std::string line;
430 int lineNum = 0;
431 bool get_erc = false;
432 bool get_etc = false;
433 int code;
434
435 while (std::getline(fdconfig, line)) {
436 lineNum++;
437 if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
438 continue;
439 }
440 std::istringstream ss(line);
441 std::string key;
442 ss >> key;
443
444 for (code = 0; NULL != opt_viper650[code]; ++code) {
445 if (key.compare(opt_viper650[code]) == 0) {
446 break;
447 }
448 }
449
450 switch (code) {
451 case 0:
452 break; // Nothing to do: camera name
453
454 case 1: {
455 ss >> erc_[0] >> erc_[1] >> erc_[2];
456
457 // Convert rotation from degrees to radians
458 erc_ = erc_ * M_PI / 180.0;
459 get_erc = true;
460 break;
461 }
462
463 case 2: {
464 ss >> etc[0] >> etc[1] >> etc[2];
465 get_etc = true;
466 break;
467 }
468
469 default:
470 throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
471 filename.c_str(), lineNum));
472 }
473 }
474
475 fdconfig.close();
476
477 // Compute the eMc matrix from the translations and rotations
478 if (get_etc && get_erc) {
479 this->set_eMc(etc_, erc_);
480 } else {
481 throw vpRobotException(vpRobotException::readingParametersError,
482 "Could not read translation and rotation "
483 "parameters from config file %s",
484 filename.c_str());
485 }
486 }
487
488 /*!
489 Get the current intrinsic camera parameters obtained by calibration.
490
491 \warning This method needs XML library to parse the file defined in
492 vpViper650::CONST_CAMERA_FILENAME and containing the camera
493 parameters.
494
495 \warning Thid method needs also an access to the files containing the
496 camera parameters in XML format. This access is available if
497 VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
498 file.
499
500 - If VISP_HAVE_VIPER650_DATA macro is defined, this method gets the camera parameters
501 from const_camera_Viper650.xml config file.
502
503 - If this macro is not defined, this method sets the camera parameters
504 to default one.
505
506 \param cam : In output, camera parameters to fill.
507 \param image_width : Image width used to compute camera calibration.
508 \param image_height : Image height used to compute camera calibration.
509
510 The code below shows how to get the camera parameters of the camera
511 attached to the robot.
512
513 \code
514 #include <visp3/core/vpImage.h>
515 #include <visp3/robot/vpRobotViper650.h>
516 #include <visp3/robot/vpViper650.h>
517 #include <visp3/sensor/vp1394TwoGrabber.h>
518
519 int main()
520 {
521 vpImage<unsigned char> I(480, 640);
522
523 #ifdef VISP_HAVE_DC1394
524 vp1394TwoGrabber g;
525
526 // Acquire an image to update image structure
527 g.acquire(I) ;
528 #endif
529
530 #ifdef VISP_HAVE_VIPER650
531 vpRobotViper650 robot;
532 #else
533 vpViper650 robot;
534 #endif
535
536 vpCameraParameters cam ;
537 // Get the intrinsic camera parameters depending on the image size
538 // Camera parameters are read from
539 // /udd/fspindle/robot/Viper650/current/include/const_camera_Viper650.xml
540 // if VISP_HAVE_VIPER650_DATA macro is defined
541 // in vpConfig.h file
542 try {
543 robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
544 }
545 catch(...) {
546 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
547 }
548 std::cout << "Camera parameters: " << cam << std::endl;
549 }
550 \endcode
551
552 \exception vpRobotException::readingParametersError : If the camera
553 parameters are not found.
554
555 */
556
getCameraParameters(vpCameraParameters & cam,const unsigned int & image_width,const unsigned int & image_height) const557 void vpViper650::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
558 const unsigned int &image_height) const
559 {
560 #if defined(VISP_HAVE_VIPER650_DATA)
561 vpXmlParserCamera parser;
562 switch (getToolType()) {
563 case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
564 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
565 << std::endl
566 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
567 if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_MARLIN_F033C_CAMERA_NAME, projModel,
568 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
569 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
570 }
571 break;
572 }
573 case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
574 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
575 << std::endl
576 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
577 if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME, projModel,
578 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
579 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
580 }
581 break;
582 }
583 case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
584 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\""
585 << std::endl
586 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
587 if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME, projModel,
588 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
589 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
590 }
591 break;
592 }
593 case vpViper650::TOOL_GENERIC_CAMERA: {
594 std::cout << "Get camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
595 << "from the XML file: \"" << vpViper650::CONST_CAMERA_FILENAME << "\"" << std::endl;
596 if (parser.parse(cam, vpViper650::CONST_CAMERA_FILENAME, vpViper650::CONST_GENERIC_CAMERA_NAME, projModel,
597 image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
598 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
599 }
600 break;
601 }
602 case vpViper650::TOOL_CUSTOM: {
603 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
604 }
605 default: {
606 vpERROR_TRACE("This error should not occur!");
607 // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
608 // "que les specs de la classe ont ete modifiee, "
609 // "et que le code n'a pas ete mis a jour "
610 // "correctement.");
611 // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
612 // "vpViper650::vpViper650ToolType, et controlez que "
613 // "tous les cas ont ete pris en compte dans la "
614 // "fonction init(camera).");
615 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
616 }
617 }
618 #else
619 // Set default parameters
620 switch (getToolType()) {
621 case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
622 // Set default intrinsic camera parameters for 640x480 images
623 if (image_width == 640 && image_height == 480) {
624 std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_MARLIN_F033C_CAMERA_NAME << "\""
625 << std::endl;
626 switch (this->projModel) {
627 case vpCameraParameters::perspectiveProjWithoutDistortion:
628 cam.initPersProjWithoutDistortion(1232.0, 1233.0, 317.7, 253.9);
629 break;
630 case vpCameraParameters::perspectiveProjWithDistortion:
631 cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
632 break;
633 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
634 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
635 break;
636 }
637 } else {
638 vpTRACE("Cannot get default intrinsic camera parameters for this image "
639 "resolution");
640 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
641 }
642 break;
643 }
644 case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
645 case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
646 // Set default intrinsic camera parameters for 640x480 images
647 if (image_width == 640 && image_height == 480) {
648 std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
649 << std::endl;
650 switch (this->projModel) {
651 case vpCameraParameters::perspectiveProjWithoutDistortion:
652 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
653 break;
654 case vpCameraParameters::perspectiveProjWithDistortion:
655 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
656 break;
657 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
658 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
659 break;
660 }
661 } else {
662 vpTRACE("Cannot get default intrinsic camera parameters for this image "
663 "resolution");
664 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
665 }
666 break;
667 }
668 case vpViper650::TOOL_GENERIC_CAMERA: {
669 // Set default intrinsic camera parameters for 640x480 images
670 if (image_width == 640 && image_height == 480) {
671 std::cout << "Get default camera parameters for camera \"" << vpViper650::CONST_GENERIC_CAMERA_NAME << "\""
672 << std::endl;
673 switch (this->projModel) {
674 case vpCameraParameters::perspectiveProjWithoutDistortion:
675 cam.initPersProjWithoutDistortion(868.0, 869.0, 314.8, 254.1);
676 break;
677 case vpCameraParameters::perspectiveProjWithDistortion:
678 cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
679 break;
680 case vpCameraParameters::ProjWithKannalaBrandtDistortion:
681 throw vpException(vpException::notImplementedError, "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
682 break;
683 }
684 } else {
685 vpTRACE("Cannot get default intrinsic camera parameters for this image "
686 "resolution");
687 throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
688 }
689 break;
690 }
691 case vpViper650::TOOL_CUSTOM: {
692 throw vpRobotException(vpRobotException::badValue, "No intrinsic parameters available for a custom tool");
693 }
694 }
695 #endif
696 return;
697 }
698
699 /*!
700 Get the current intrinsic camera parameters obtained by calibration.
701
702 \warning This method needs XML library to parse the file defined in
703 vpViper650::CONST_CAMERA_FILENAME and containing the camera
704 parameters.
705
706 \warning Thid method needs also an access to the files containing the
707 camera parameters in XML format. This access is available if
708 VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
709 file.
710
711 - If VISP_HAVE_VIPER650_DATA macro is defined, this method gets the camera parameters
712 from const_camera_Viper650.xml config file.
713
714 - If these two macros are not defined, this method set the camera parameters
715 to default one.
716
717 \param cam : In output, camera parameters to fill.
718 \param I : A B&W image send by the current camera in use.
719
720 \code
721 #include <visp3/core/vpImage.h>
722 #include <visp3/robot/vpRobotViper650.h>
723 #include <visp3/robot/vpViper650.h>
724 #include <visp3/sensor/vp1394TwoGrabber.h>
725
726 int main()
727 {
728 vpImage<unsigned char> I(480, 640);
729
730 #ifdef VISP_HAVE_DC1394
731 vp1394TwoGrabber g;
732
733 // Acquire an image to update image structure
734 g.acquire(I) ;
735 #endif
736
737 #ifdef VISP_HAVE_VIPER650
738 vpRobotViper650 robot;
739 #else
740 vpViper650 robot;
741 #endif
742
743 vpCameraParameters cam ;
744 // Get the intrinsic camera parameters depending on the image size
745 try {
746 robot.getCameraParameters (cam, I);
747 }
748 catch(...) {
749 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
750 }
751 std::cout << "Camera parameters: " << cam << std::endl;
752 }
753 \endcode
754
755 \exception vpRobotException::readingParametersError : If the camera
756 parameters are not found.
757
758 */
getCameraParameters(vpCameraParameters & cam,const vpImage<unsigned char> & I) const759 void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const
760 {
761 getCameraParameters(cam, I.getWidth(), I.getHeight());
762 }
763 /*!
764 \brief Get the current intrinsic camera parameters obtained by calibration.
765
766 \warning This method needs XML library to parse the file defined in
767 vpViper650::CONST_CAMERA_FILENAME and containing the camera
768 parameters.
769
770 \warning Thid method needs also an access to the files containing the camera
771 parameters in XML format. This access is available if
772 VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
773 file.
774
775 - If VISP_HAVE_VIPER650_DATA macro is defined, this method gets the camera parameters
776 from const_camera_Viper650.xml config file.
777
778 - If these two macros are not defined, this method set the camera parameters
779 to default one.
780
781 \param cam : In output, camera parameters to fill.
782 \param I : A color image send by the current camera in use.
783
784 \code
785 #include <visp3/core/vpImage.h>
786 #include <visp3/robot/vpRobotViper650.h>
787 #include <visp3/robot/vpViper650.h>
788 #include <visp3/sensor/vp1394TwoGrabber.h>
789
790 int main()
791 {
792 vpImage<vpRGBa> I(480, 640);
793
794 #ifdef VISP_HAVE_DC1394
795 vp1394TwoGrabber g;
796
797 // Acquire an image to update image structure
798 g.acquire(I) ;
799 #endif
800
801 #ifdef VISP_HAVE_VIPER650
802 vpRobotViper650 robot;
803 #else
804 vpViper650 robot;
805 #endif
806
807 vpCameraParameters cam ;
808 // Get the intrinsic camera parameters depending on the image size
809 try {
810 robot.getCameraParameters (cam, I);
811 }
812 catch(...) {
813 std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
814 }
815 std::cout << "Camera parameters: " << cam << std::endl;
816 }
817 \endcode
818
819 \exception vpRobotException::readingParametersError : If the camera
820 parameters are not found.
821
822 */
823
getCameraParameters(vpCameraParameters & cam,const vpImage<vpRGBa> & I) const824 void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const
825 {
826 getCameraParameters(cam, I.getWidth(), I.getHeight());
827 }
828