00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #include "../include/cob_camera_sensors/StdAfx.h"
00054 #ifdef __LINUX__
00055 #include "cob_camera_sensors/Swissranger.h"
00056 #include "cob_vision_utils/VisionUtils.h"
00057 #include "tinyxml.h"
00058 #else
00059 #include "cob_driver/cob_camera_sensors/common/include/cob_camera_sensors/Swissranger.h"
00060 #include "cob_common/cob_vision_utils/common/include/cob_vision_utils/VisionUtils.h"
00061 #include "cob_vision/windows/src/extern/TinyXml/tinyxml.h"
00062 #endif
00063
00064 using namespace ipa_CameraSensors;
00065
00066 __DLL_LIBCAMERASENSORS__ AbstractRangeImagingSensorPtr ipa_CameraSensors::CreateRangeImagingSensor_Swissranger()
00067 {
00068 return AbstractRangeImagingSensorPtr(new Swissranger());
00069 }
00070
00071 Swissranger::Swissranger()
00072 {
00073 m_initialized = false;
00074 m_open = false;
00075
00076 m_SRCam = 0;
00077 m_DataBuffer = 0;
00078 m_BufferSize = 1;
00079
00080 m_CoeffsInitialized = false;
00081 }
00082
00083
00084 Swissranger::~Swissranger()
00085 {
00086 if (isOpen())
00087 {
00088 Close();
00089 }
00090 }
00091
00092 int ipa_CameraSensors::LibMesaCallback(SRCAM srCam, unsigned int msg, unsigned int param, void* data)
00093 {
00094 switch(msg)
00095 {
00096 case CM_MSG_DISPLAY:
00097 {
00098 if (param==MC_ETH)
00099 {
00100
00101 return 0;
00102 }
00103 else
00104 {
00105 return SR_GetDefaultCallback()(0,msg,param,data);
00106 }
00107 break;
00108 }
00109 default:
00110 {
00111
00112 return SR_GetDefaultCallback()(0,msg,param,data);
00113 }
00114 }
00115 return 0;
00116 }
00117
00118
00119 unsigned long Swissranger::Init(std::string directory, int cameraIndex)
00120 {
00121 if (isInitialized())
00122 {
00123 return (RET_OK | RET_CAMERA_ALREADY_INITIALIZED);
00124 }
00125
00126 m_CameraType = ipa_CameraSensors::CAM_SWISSRANGER;
00127
00128
00129 if (LoadParameters((directory + "cameraSensorsIni.xml").c_str(), cameraIndex) & RET_FAILED)
00130 {
00131 std::cerr << "ERROR - Swissranger::Init:" << std::endl;
00132 std::cerr << "\t ... Parsing xml configuration file failed." << std::endl;
00133 return (RET_FAILED | RET_INIT_CAMERA_FAILED);
00134 }
00135
00136 m_CoeffsInitialized = true;
00137
00138
00139 SR_SetCallback(LibMesaCallback);
00140
00141 if (m_CalibrationMethod == MATLAB)
00142 {
00143
00144 std::string filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA0.xml";
00145 CvMat* c_mat = (CvMat*)cvLoad(filename.c_str());
00146 if (! c_mat)
00147 {
00148 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00149 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA0.txt" << "." << std::endl;
00150 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00151 m_CoeffsInitialized = false;
00152
00153 }
00154 else
00155 {
00156 m_CoeffsA0 = c_mat;
00157 cvReleaseMat(&c_mat);
00158 }
00159
00160 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA1.xml";
00161 c_mat = (CvMat*)cvLoad(filename.c_str());
00162 if (! c_mat)
00163 {
00164 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00165 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA1.txt" << "." << std::endl;
00166 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00167 m_CoeffsInitialized = false;
00168
00169 }
00170 else
00171 {
00172 m_CoeffsA1 = c_mat;
00173 cvReleaseMat(&c_mat);
00174 }
00175
00176 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA2.xml";
00177 c_mat = (CvMat*)cvLoad(filename.c_str());
00178 if (! c_mat)
00179 {
00180 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00181 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA2.txt" << "." << std::endl;
00182 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00183 m_CoeffsInitialized = false;
00184
00185 }
00186 else
00187 {
00188 m_CoeffsA2 = c_mat;
00189 cvReleaseMat(&c_mat);
00190 }
00191
00192 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA3.xml";
00193 c_mat = (CvMat*)cvLoad(filename.c_str());
00194 if (! c_mat)
00195 {
00196 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00197 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA3.txt" << "." << std::endl;
00198 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00199 m_CoeffsInitialized = false;
00200
00201 }
00202 else
00203 {
00204 m_CoeffsA3 = c_mat;
00205 cvReleaseMat(&c_mat);
00206 }
00207
00208 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA4.xml";
00209 c_mat = (CvMat*)cvLoad(filename.c_str());
00210 if (! c_mat)
00211 {
00212 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00213 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA4.txt" << "." << std::endl;
00214 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00215 m_CoeffsInitialized = false;
00216
00217 }
00218 else
00219 {
00220 m_CoeffsA4 = c_mat;
00221 cvReleaseMat(&c_mat);
00222 }
00223
00224 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA5.xml";
00225 c_mat = (CvMat*)cvLoad(filename.c_str());
00226 if (! c_mat)
00227 {
00228 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00229 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA5.txt" << "." << std::endl;
00230 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00231 m_CoeffsInitialized = false;
00232
00233 }
00234 else
00235 {
00236 m_CoeffsA5 = c_mat;
00237 cvReleaseMat(&c_mat);
00238 }
00239
00240 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA6.xml";
00241 c_mat = (CvMat*)cvLoad(filename.c_str());
00242 if (! c_mat)
00243 {
00244 std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00245 std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA6.txt" << "." << std::endl;
00246 std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00247 m_CoeffsInitialized = false;
00248
00249 }
00250 else
00251 {
00252 m_CoeffsA6 = c_mat;
00253 cvReleaseMat(&c_mat);
00254 }
00255 }
00256
00257
00258 m_initialized = true;
00259 m_GrayImageAcquireCalled = false;
00260
00261 return RET_OK;
00262 }
00263
00264
00265 unsigned long Swissranger::Open()
00266 {
00267 if (!isInitialized())
00268 {
00269 return (RET_FAILED | RET_CAMERA_NOT_INITIALIZED);
00270 }
00271
00272 if (isOpen())
00273 {
00274 return (RET_OK | RET_CAMERA_ALREADY_OPEN);
00275 }
00276
00277 std::string sInterface = "";
00278 m_RangeCameraParameters.m_Interface.clear();
00279 m_RangeCameraParameters.m_Interface.seekg(0);
00280 m_RangeCameraParameters.m_Interface >> sInterface;
00281
00282 if (sInterface == "USB")
00283 {
00284 if(SR_OpenUSB(&m_SRCam, 0)<=0)
00285 {
00286 std::cerr << "ERROR - Swissranger::Open():" << std::endl;
00287 std::cerr << "\t ... Could not open swissranger camera on USB port" << std::endl;
00288 std::cerr << "\t ... Unplug and Replugin camera power cable.\n";
00289 return RET_FAILED;
00290 }
00291 }
00292 else if (sInterface == "ETHERNET")
00293 {
00294 std::string sIP = "";
00295 m_RangeCameraParameters.m_IP.clear();
00296 m_RangeCameraParameters.m_IP.seekg(0);
00297 m_RangeCameraParameters.m_IP >> sIP;
00298 if(SR_OpenETH(&m_SRCam, sIP.c_str())<=0)
00299 {
00300 std::cerr << "ERROR - Swissranger::Open():" << std::endl;
00301 std::cerr << "\t ... Could not open swissranger camera on ETHERNET port" << std::endl;
00302 std::cerr << "\t ... with ip '" << sIP << "'." << std::endl;
00303 std::cerr << "\t ... Unplug and Replugin camera power cable to fix problem\n";
00304 return RET_FAILED;
00305 }
00306 }
00307 else
00308 {
00309 std::cerr << "ERROR - Swissranger::Open():" << std::endl;
00310 std::cerr << "\t ... Unknown interface type '" << sInterface << "'" << std::endl;
00311 return RET_FAILED;
00312 }
00313
00314
00315
00316
00317
00318
00319 if (SetParameters() & ipa_CameraSensors::RET_FAILED)
00320 {
00321 std::cerr << "ERROR - AVTPikeCam::Open:" << std::endl;
00322 std::cerr << "\t ... Could not set parameters" << std::endl;
00323 return RET_FAILED;
00324 }
00325
00326 std::cout << "**************************************************" << std::endl;
00327 std::cout << "Swissranger::Open: Swissranger camera device OPEN" << std::endl;
00328 std::cout << "**************************************************" << std::endl << std::endl;
00329 m_open = true;
00330
00331 return RET_OK;
00332 }
00333
00334
00335 unsigned long Swissranger::Close()
00336 {
00337 if (!isOpen())
00338 {
00339 return (RET_OK);
00340 }
00341
00342 if(SR_Close(m_SRCam)<0)
00343 {
00344 std::cout << "ERROR - Swissranger::Close():" << std::endl;
00345 std::cerr << "\t ... Could not close swissranger camera." << std::endl;
00346 return RET_FAILED;
00347 }
00348 m_SRCam = 0;
00349
00350 m_open = false;
00351 return RET_OK;
00352
00353 }
00354
00355
00356 unsigned long Swissranger::SetProperty(t_cameraProperty* cameraProperty)
00357 {
00358 if (!m_SRCam)
00359 {
00360 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00361 }
00362
00363 int err = 0;
00364 switch (cameraProperty->propertyID)
00365 {
00366 case PROP_AMPLITUDE_THRESHOLD:
00367 if (cameraProperty->propertyType & ipa_CameraSensors::TYPE_SPECIAL)
00368 {
00369 if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_AUTO)
00370 {
00371 unsigned short val = 0;
00372 err =SR_SetAmplitudeThreshold(m_SRCam, val);
00373 if(err<0)
00374 {
00375 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00376 std::cerr << "\t ... Could not set amplitude threshold to AUTO mode" << std::endl;
00377 return RET_FAILED;
00378 }
00379 }
00380 else if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_DEFAULT)
00381 {
00382
00383 }
00384 else
00385 {
00386 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00387 std::cerr << "\t ... Special value 'VALUE_AUTO' or 'VALUE_DEFAULT' expected." << std::endl;
00388 return RET_FAILED;
00389 }
00390 }
00391 else if (cameraProperty->propertyType & (ipa_CameraSensors::TYPE_SHORT | ipa_CameraSensors::TYPE_UNSIGNED))
00392 {
00393 err =SR_SetAmplitudeThreshold(m_SRCam, cameraProperty->u_shortData);
00394 if(err<0)
00395 {
00396 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00397 std::cerr << "\t ... Could not set amplitude threshold to AUTO mode" << std::endl;
00398 return RET_FAILED;
00399 }
00400 }
00401 else
00402 {
00403 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00404 std::cerr << "\t ... Wrong property type. '(TYPE_SHORT|TYPE_UNSIGNED)' or special value 'TYPE_SPECIAL' expected." << std::endl;
00405 return RET_FAILED;
00406 }
00407 break;
00408 case PROP_INTEGRATION_TIME:
00409 if (cameraProperty->propertyType & ipa_CameraSensors::TYPE_SPECIAL)
00410 {
00411 if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_AUTO)
00412 {
00413 err = SR_SetAutoExposure(m_SRCam, 1, 150, 5, 40);
00414 if(err<0)
00415 {
00416 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00417 std::cerr << "\t ... Could not set integration time to AUTO mode" << std::endl;
00418 return RET_FAILED;
00419 }
00420 }
00421 else if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_DEFAULT)
00422 {
00423
00424 }
00425 else
00426 {
00427 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00428 std::cerr << "\t ... Special value 'VALUE_AUTO' or 'VALUE_DEFAULT' expected." << std::endl;
00429 return RET_FAILED;
00430 }
00431 }
00432 else if (cameraProperty->propertyType & (ipa_CameraSensors::TYPE_CHARACTER | ipa_CameraSensors::TYPE_UNSIGNED))
00433 {
00434 err = SR_SetAutoExposure(m_SRCam, 0xff, 150, 5, 70);
00435 if(err<0)
00436 {
00437 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00438 std::cerr << "\t ... Could not turn off auto exposure" << std::endl;
00439 return RET_FAILED;
00440 }
00441 err = SR_SetIntegrationTime(m_SRCam, cameraProperty->u_charData);
00442 if(err<0)
00443 {
00444 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00445 std::cerr << "\t ... Could not set amplitude threshold to '" << cameraProperty->u_charData << "'" << std::endl;
00446 return RET_FAILED;
00447 }
00448 }
00449 else
00450 {
00451 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00452 std::cerr << "\t ... Wrong property type. '(TYPE_LONG|TYPE_UNSIGNED)' or special value 'TYPE_SPECIAL' expected." << std::endl;
00453 return RET_FAILED;
00454 }
00455 break;
00456 case PROP_MODULATION_FREQUENCY:
00457 if (cameraProperty->propertyType & ipa_CameraSensors::TYPE_SPECIAL)
00458 {
00459 if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_AUTO)
00460 {
00461 err = SR_SetModulationFrequency(m_SRCam, MF_LAST);
00462 if(err<0)
00463 {
00464 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00465 std::cerr << "\t ... Could not set modulation frequency to AUTO mode" << std::endl;
00466 return RET_FAILED;
00467 }
00468 }
00469 else if(cameraProperty->specialValue == ipa_CameraSensors::VALUE_DEFAULT)
00470 {
00471
00472 }
00473 else
00474 {
00475 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00476 std::cerr << "\t ... Special value 'VALUE_AUTO' or 'VALUE_DEFAULT' expected." << std::endl;
00477 return RET_FAILED;
00478 }
00479 }
00480 else if (cameraProperty->propertyType & (ipa_CameraSensors::TYPE_STRING))
00481 {
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492 if (cameraProperty->stringData == "MF_40MHz")
00493 {
00494 err = SR_SetModulationFrequency(m_SRCam, MF_40MHz);
00495 }
00496 else if (cameraProperty->stringData == "MF_30MHz")
00497 {
00498 err = SR_SetModulationFrequency(m_SRCam, MF_30MHz);
00499 }
00500 else if (cameraProperty->stringData == "MF_21MHz")
00501 {
00502 err = SR_SetModulationFrequency(m_SRCam, MF_21MHz);
00503 }
00504 else if (cameraProperty->stringData == "MF_20MHz")
00505 {
00506 err = SR_SetModulationFrequency(m_SRCam, MF_20MHz);
00507 }
00508 else if (cameraProperty->stringData == "MF_19MHz")
00509 {
00510 err = SR_SetModulationFrequency(m_SRCam, MF_19MHz);
00511 }
00512 else if (cameraProperty->stringData == "MF_60MHz")
00513 {
00514 err = SR_SetModulationFrequency(m_SRCam, MF_60MHz);
00515 }
00516 else if (cameraProperty->stringData == "MF_15MHz")
00517 {
00518 err = SR_SetModulationFrequency(m_SRCam, MF_15MHz);
00519 }
00520 else if (cameraProperty->stringData == "MF_10MHz")
00521 {
00522 err = SR_SetModulationFrequency(m_SRCam, MF_10MHz);
00523 }
00524 else if (cameraProperty->stringData == "MF_29MHz")
00525 {
00526 err = SR_SetModulationFrequency(m_SRCam, MF_29MHz);
00527 }
00528 else if (cameraProperty->stringData == "MF_31MHz")
00529 {
00530 err = SR_SetModulationFrequency(m_SRCam, MF_31MHz);
00531 }
00532 else
00533 {
00534 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00535 std::cerr << "\t ... Modulation frequency " << cameraProperty->stringData << " unknown" << std::endl;
00536 }
00537
00538 if(err<0)
00539 {
00540 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00541 std::cerr << "\t ... Could not set modulation frequency " << cameraProperty->stringData << std::endl;
00542 return RET_FAILED;
00543 }
00544
00545 }
00546 else
00547 {
00548 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00549 std::cerr << "\t ... Wrong property type. '(TYPE_LONG|TYPE_UNSIGNED)' or special value 'TYPE_SPECIAL' expected." << std::endl;
00550 return RET_FAILED;
00551 }
00552 break;
00553 case PROP_ACQUIRE_MODE:
00554 if (cameraProperty->propertyType & ipa_CameraSensors::TYPE_INTEGER)
00555 {
00556 err = SR_SetMode(m_SRCam, cameraProperty->integerData);
00557 if(err<0)
00558 {
00559 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00560 std::cerr << "\t ... Could not set acquire mode" << std::endl;
00561 return RET_FAILED;
00562 }
00563 }
00564 else
00565 {
00566 std::cerr << "ERROR - Swissranger::SetProperty:" << std::endl;
00567 std::cerr << "\t ... Wrong property type. 'TYPE_INTEGER' expected." << std::endl;
00568 return RET_FAILED;
00569 }
00570 break;
00571 default:
00572 std::cout << "Swissranger::SetProperty: Property " << cameraProperty->propertyID << " unspecified.\n";
00573 return RET_FAILED;
00574 break;
00575 }
00576
00577 return RET_OK;
00578 }
00579
00580
00581 unsigned long Swissranger::SetPropertyDefaults()
00582 {
00583 return RET_FUNCTION_NOT_IMPLEMENTED;
00584 }
00585
00586
00587 unsigned long Swissranger::GetProperty(t_cameraProperty* cameraProperty)
00588 {
00589 switch (cameraProperty->propertyID)
00590 {
00591 case PROP_DMA_BUFFER_SIZE:
00592 cameraProperty->u_integerData = m_BufferSize;
00593 return RET_OK;
00594 break;
00595 case PROP_AMPLITUDE_THRESHOLD:
00596 if (isOpen())
00597 {
00598 cameraProperty->u_shortData = SR_GetAmplitudeThreshold(m_SRCam);
00599 cameraProperty->propertyType = (TYPE_UNSIGNED | TYPE_SHORT);
00600 }
00601 else
00602 {
00603 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00604 }
00605 break;
00606
00607 case PROP_INTEGRATION_TIME:
00608 if (isOpen())
00609 {
00610 cameraProperty->u_charData = SR_GetIntegrationTime(m_SRCam);
00611 cameraProperty->propertyType = (TYPE_UNSIGNED | TYPE_CHARACTER);
00612 }
00613 else
00614 {
00615 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00616 }
00617 break;
00618
00619 case PROP_ACQUIRE_MODE:
00620 if (isOpen())
00621 {
00622 cameraProperty->integerData = SR_GetMode(m_SRCam);
00623 cameraProperty->propertyType = TYPE_INTEGER;
00624 }
00625 else
00626 {
00627 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00628 }
00629 break;
00630
00631 case PROP_CAMERA_RESOLUTION:
00632 if (isOpen())
00633 {
00634 cameraProperty->cameraResolution.xResolution = (int)SR_GetCols(m_SRCam);
00635 cameraProperty->cameraResolution.yResolution = (int)SR_GetRows(m_SRCam);
00636 cameraProperty->propertyType = TYPE_CAMERA_RESOLUTION;
00637 }
00638 else
00639 {
00640 cameraProperty->cameraResolution.xResolution = 176;
00641 cameraProperty->cameraResolution.yResolution = 144;
00642 cameraProperty->propertyType = TYPE_CAMERA_RESOLUTION;
00643 }
00644 break;
00645
00646 default:
00647 std::cout << "ERROR - Swissranger::GetProperty:" << std::endl;
00648 std::cout << "\t ... Property " << cameraProperty->propertyID << " unspecified.";
00649 return RET_FAILED;
00650 break;
00651
00652 }
00653
00654 return RET_OK;
00655 }
00656
00657
00658
00659
00660 unsigned long Swissranger::AcquireImages(cv::Mat* rangeImage, cv::Mat* grayImage, cv::Mat* cartesianImage,
00661 bool getLatestFrame, bool undistort, ipa_CameraSensors::t_ToFGrayImageType grayImageType)
00662 {
00663 char* rangeImageData = 0;
00664 char* grayImageData = 0;
00665 char* cartesianImageData = 0;
00666 int widthStepRange = -1;
00667 int widthStepGray = -1;
00668 int widthStepCartesian = -1;
00669
00670 int width = -1;
00671 int height = -1;
00672 ipa_CameraSensors::t_cameraProperty cameraProperty;
00673 cameraProperty.propertyID = PROP_CAMERA_RESOLUTION;
00674 GetProperty(&cameraProperty);
00675 width = cameraProperty.cameraResolution.xResolution;
00676 height = cameraProperty.cameraResolution.yResolution;
00677
00678 if(rangeImage)
00679 {
00680 rangeImage->create(height, width, CV_32FC(1));
00681 rangeImageData = rangeImage->ptr<char>(0);
00682 widthStepRange = rangeImage->step;
00683 }
00684
00685 if(grayImage)
00686 {
00687 grayImage->create(height, width, CV_32FC(1));
00688 grayImageData = grayImage->ptr<char>(0);
00689 widthStepGray = grayImage->step;
00690 }
00691
00692 if(cartesianImage)
00693 {
00694 cartesianImage->create(height, width, CV_32FC(3));
00695 cartesianImageData = cartesianImage->ptr<char>(0);
00696 widthStepCartesian = cartesianImage->step;
00697 }
00698
00699 if (!rangeImage && !grayImage && !cartesianImage)
00700 {
00701 return RET_OK;
00702 }
00703
00704 return AcquireImages(widthStepRange, widthStepGray, widthStepCartesian, rangeImageData, grayImageData, cartesianImageData, getLatestFrame, undistort, grayImageType);
00705
00706 }
00707
00708
00709 unsigned long Swissranger::AcquireImages(int widthStepRange, int widthStepGray, int widthStepCartesian, char* rangeImageData, char* grayImageData, char* cartesianImageData,
00710 bool getLatestFrame, bool undistort, ipa_CameraSensors::t_ToFGrayImageType grayImageType)
00711 {
00713
00715 if (!m_open)
00716 {
00717 std::cerr << "ERROR - Swissranger::AcquireImages:" << std::endl;
00718 std::cerr << "t ... Camera not open." << std::endl;
00719 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00720 }
00721
00722
00723
00724
00725
00726
00727 int width = -1;
00728 int height = -1;
00729 ipa_CameraSensors::t_cameraProperty cameraProperty;
00730 cameraProperty.propertyID = PROP_CAMERA_RESOLUTION;
00731 GetProperty(&cameraProperty);
00732 width = cameraProperty.cameraResolution.xResolution;
00733 height = cameraProperty.cameraResolution.yResolution;
00734
00735 unsigned int bytesRead = 0;
00736 bytesRead = SR_Acquire(m_SRCam);
00737 if(bytesRead <= 0)
00738 {
00739 std::cerr << "ERROR - Swissranger::AcquireImages:" << std::endl;
00740 std::cerr << "\t ... Could not acquire image!" << std::endl;
00741 return RET_FAILED;
00742 }
00743
00744 if (getLatestFrame == true)
00745 {
00746 bytesRead = SR_Acquire(m_SRCam);
00747 if(bytesRead <= 0)
00748 {
00749 std::cerr << "ERROR - Swissranger::AcquireImages:" << std::endl;
00750 std::cerr << "\t ... Could not acquire image!" << std::endl;
00751 return RET_FAILED;
00752 }
00753 }
00754 WORD* pixels =(WORD*) SR_GetImage(m_SRCam, 0);
00756
00758 if (rangeImageData)
00759 {
00760 int imageStep = -1;
00761 float* f_ptr = 0;
00762
00763
00764 for(unsigned int row=0; row<(unsigned int)height; row++)
00765 {
00766 imageStep = row*width;
00767 f_ptr = (float*)(rangeImageData + row*widthStepRange);
00768
00769 for (unsigned int col=0; col<(unsigned int)width; col++)
00770 {
00771 f_ptr[col] = (float)(pixels[imageStep + col]);
00772 }
00773 }
00774
00775 if (undistort)
00776 {
00777 cv::Mat undistortedData(height, width, CV_32FC(1), (float*) rangeImageData);
00778 cv::Mat distortedData;
00779
00780 assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00781 cv::remap(distortedData, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00782 }
00783
00784 }
00785
00787
00788
00790 if(grayImageData)
00791 {
00792 if (grayImageType == ipa_CameraSensors::INTENSITY_32F1 &&
00793 m_GrayImageAcquireCalled == false)
00794 {
00795 std::cout << "WARNING - Swissranger::AcquireImages:" << std::endl;
00796 std::cout << "\t ... Intensity image for swissranger not available" << std::endl;
00797 std::cout << "\t ... falling back to amplitude image" << std::endl;
00798 m_GrayImageAcquireCalled = true;
00799 }
00800
00801 int imageSize = width*height;
00802 int imageStep = 0;
00803 float* f_ptr = 0;
00804
00805 for(unsigned int row=0; row<(unsigned int)height-1; row++)
00806 {
00807 imageStep = imageSize+row*width;
00808 f_ptr = (float*)(grayImageData + row*widthStepGray);
00809
00810 for (unsigned int col=0; col<(unsigned int)width-1; col++)
00811 {
00812 f_ptr[col] = (float)(pixels[imageStep+col]);
00813 }
00814 }
00815
00816 if (undistort)
00817 {
00818 cv::Mat undistortedData( height, width, CV_32FC1, (float*) grayImageData);
00819 cv::Mat distortedData;
00820
00821 assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00822 cv::remap(distortedData, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00823 }
00824
00825 }
00826
00828
00830 if(cartesianImageData)
00831 {
00832 float x = -1;
00833 float y = -1;
00834 float zRaw = -1;
00835 float* zCalibratedPtr = 0;
00836 float zCalibrated = -1;
00837 float* f_ptr = 0;
00838
00839 if(m_CalibrationMethod==MATLAB)
00840 {
00841 if (m_CoeffsInitialized)
00842 {
00843
00844 cv::Mat distortedData( height, width, CV_32FC1 );
00845 for(unsigned int row=0; row<(unsigned int)height; row++)
00846 {
00847 f_ptr = distortedData.ptr<float>(row);
00848 for (unsigned int col=0; col<(unsigned int)width; col++)
00849 {
00850 zRaw = (float)(pixels[width*row + col]);
00851 GetCalibratedZMatlab(col, row, zRaw, zCalibrated);
00852 f_ptr[col] = zCalibrated;
00853 }
00854 }
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865 cv::Mat undistortedData;
00866 assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00867 cv::remap(distortedData, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878 for(unsigned int row=0; row<(unsigned int)height; row++)
00879 {
00880 zCalibratedPtr = undistortedData.ptr<float>(row);
00881 f_ptr = (float*)(cartesianImageData + row*widthStepCartesian);
00882
00883 for (unsigned int col=0; col<(unsigned int)width; col++)
00884 {
00885 int colTimes3 = 3*col;
00886 GetCalibratedXYMatlab(col, row, zCalibratedPtr[col], x, y);
00887
00888 f_ptr[colTimes3] = x;
00889 f_ptr[colTimes3 + 1] = y;
00890 f_ptr[colTimes3 + 2] = zCalibratedPtr[col];
00891 }
00892 }
00893 }
00894 else
00895 {
00896 std::cerr << "ERROR - Swissranger::AcquireImages: \n";
00897 std::cerr << "\t ... At least one of m_CoeffsA0 ... m_CoeffsA6 not initialized.\n";
00898 return RET_FAILED;
00899 }
00900
00901 }
00902 else if(m_CalibrationMethod==MATLAB_NO_Z)
00903 {
00904 SR_CoordTrfFlt(m_SRCam, m_X, m_Y, m_Z, sizeof(float), sizeof(float), sizeof(float));
00905
00906 cv::Mat distortedData( height, width, CV_32FC1 );
00907 for(unsigned int row=0; row<(unsigned int)height; row++)
00908 {
00909 f_ptr = distortedData.ptr<float>(row);
00910 for (unsigned int col=0; col<(unsigned int)width; col++)
00911 {
00912 GetCalibratedZSwissranger(col, row, width, zCalibrated);
00913 f_ptr[col] = zCalibrated;
00914 }
00915 }
00916
00917
00918 cv::Mat undistortedData;
00919 assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00920 cv::remap(distortedData, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00921
00922
00923 for(unsigned int row=0; row<(unsigned int)height; row++)
00924 {
00925 zCalibratedPtr = undistortedData.ptr<float>(row);
00926 f_ptr = (float*)(cartesianImageData + row*widthStepCartesian);
00927
00928 for (unsigned int col=0; col<(unsigned int)width; col++)
00929 {
00930 int colTimes3 = 3*col;
00931 GetCalibratedXYMatlab(col, row, zCalibratedPtr[col], x, y);
00932
00933 f_ptr[colTimes3] = x;
00934 f_ptr[colTimes3 + 1] = y;
00935 f_ptr[colTimes3 + 2] = zCalibratedPtr[col];
00936 }
00937 }
00938 }
00939 else if(m_CalibrationMethod==NATIVE)
00940 {
00941 SR_CoordTrfFlt(m_SRCam, m_X, m_Y, m_Z, sizeof(float), sizeof(float), sizeof(float));
00942
00943 for(unsigned int row=0; row<(unsigned int)height; row++)
00944 {
00945 f_ptr = (float*)(cartesianImageData + row*widthStepCartesian);
00946
00947 for (unsigned int col=0; col<(unsigned int)width; col++)
00948 {
00949 int colTimes3 = 3*col;
00950
00951 GetCalibratedZSwissranger(col, row, width, zCalibrated);
00952 GetCalibratedXYSwissranger(col, row, width, x, y);
00953
00954 f_ptr[colTimes3] = x;
00955 f_ptr[colTimes3 + 1] = y;
00956 f_ptr[colTimes3 + 2] = zCalibrated;
00957 }
00958 }
00959 }
00960 else
00961 {
00962 std::cerr << "ERROR - Swissranger::AcquireImages:" << std::endl;
00963 std::cerr << "\t ... Calibration method unknown.\n";
00964 return RET_FAILED;
00965 }
00966 }
00967 return RET_OK;
00968 }
00969
00970 unsigned long Swissranger::SaveParameters(const char* filename)
00971 {
00972 return RET_FUNCTION_NOT_IMPLEMENTED;
00973 }
00974
00975 unsigned long Swissranger::GetCalibratedZMatlab(int u, int v, float zRaw, float& zCalibrated)
00976 {
00977
00978 double c[7] = {m_CoeffsA0.at<double>(v,u), m_CoeffsA1.at<double>(v,u), m_CoeffsA2.at<double>(v,u),
00979 m_CoeffsA3.at<double>(v,u), m_CoeffsA4.at<double>(v,u), m_CoeffsA5.at<double>(v,u), m_CoeffsA6.at<double>(v,u)};
00980 double y = 0;
00981 ipa_Utils::EvaluatePolynomial((double) zRaw, 6, &c[0], &y);
00982 zCalibrated = (float) y;
00983
00984 return RET_OK;
00985 }
00986
00987
00988 unsigned long Swissranger::GetCalibratedZSwissranger(int u, int v, int width, float& zCalibrated)
00989 {
00990 zCalibrated = (float) m_Z[v*width + u];
00991
00992 return RET_OK;
00993 }
00994
00995
00996 unsigned long Swissranger::GetCalibratedXYMatlab(int u, int v, float z, float& x, float& y)
00997 {
00998
00999 z *= 1000;
01000
01001
01002 double fx, fy, cx, cy;
01003
01004 fx = m_intrinsicMatrix.at<double>(0, 0);
01005 fy = m_intrinsicMatrix.at<double>(1, 1);
01006
01007 cx = m_intrinsicMatrix.at<double>(0, 2);
01008 cy = m_intrinsicMatrix.at<double>(1, 2);
01009
01010
01011 if (fx == 0)
01012 {
01013 std::cerr << "ERROR - Swissranger::GetCalibratedXYZ:" << std::endl;
01014 std::cerr << "\t ... fx is 0.\n";
01015 return RET_FAILED;
01016 }
01017 x = (float) (z*(u-cx)/fx) ;
01018
01019
01020 if (fy == 0)
01021 {
01022 std::cerr << "ERROR - Swissranger::GetCalibratedXYZ:" << std::endl;
01023 std::cerr << "\t ... fy is 0.\n";
01024 return RET_FAILED;
01025 }
01026 y = (float) (z*(v-cy)/fy);
01027
01028
01029 x /= 1000;
01030 y /= 1000;
01031
01032 return RET_OK;
01033 }
01034
01035 unsigned long Swissranger::GetCalibratedXYSwissranger(int u, int v, int width, float& x, float& y)
01036 {
01037
01038 int i = v*width + u;
01039 x = (float)m_X[i];
01040 y = (float)m_Y[i];
01041
01042 return RET_OK;
01043 }
01044
01045 unsigned long Swissranger::SetParameters()
01046 {
01047 ipa_CameraSensors::t_cameraProperty cameraProperty;
01048
01049
01050
01051
01052
01053 cameraProperty.propertyID = ipa_CameraSensors::PROP_AMPLITUDE_THRESHOLD;
01054 std::string sAmplitudeThreshold = "";
01055 m_RangeCameraParameters.m_AmplitudeThreshold.clear();
01056 m_RangeCameraParameters.m_AmplitudeThreshold.seekg(0);
01057 m_RangeCameraParameters.m_AmplitudeThreshold >> sAmplitudeThreshold;
01058 if (sAmplitudeThreshold == "AUTO")
01059 {
01060 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01061 cameraProperty.specialValue = VALUE_AUTO;
01062 }
01063 else if (sAmplitudeThreshold == "DEFAULT")
01064 {
01065 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01066 cameraProperty.specialValue = VALUE_DEFAULT;
01067 }
01068 else
01069 {
01070 cameraProperty.propertyType = (ipa_CameraSensors::TYPE_UNSIGNED | ipa_CameraSensors::TYPE_SHORT);
01071 m_RangeCameraParameters.m_AmplitudeThreshold.clear();
01072 m_RangeCameraParameters.m_AmplitudeThreshold.seekg(0);
01073 m_RangeCameraParameters.m_AmplitudeThreshold >> cameraProperty.u_shortData;
01074 }
01075
01076 if (SetProperty(&cameraProperty) & ipa_CameraSensors::RET_FAILED)
01077 {
01078 std::cout << "WARNING - Swissranger::SetParameters:" << std::endl;
01079 std::cout << "\t ... Could not set amplitude threshold" << std::endl;
01080 }
01081
01082
01083
01084
01085 cameraProperty.propertyID = ipa_CameraSensors::PROP_INTEGRATION_TIME;
01086 std::string sIntegrationTime = "";
01087 m_RangeCameraParameters.m_IntegrationTime.clear();
01088 m_RangeCameraParameters.m_IntegrationTime.seekg(0);
01089 m_RangeCameraParameters.m_IntegrationTime >> sIntegrationTime;
01090 if (sIntegrationTime == "AUTO")
01091 {
01092 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01093 cameraProperty.specialValue = VALUE_AUTO;
01094 }
01095 else if (sIntegrationTime == "DEFAULT")
01096 {
01097 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01098 cameraProperty.specialValue = VALUE_DEFAULT;
01099 }
01100 else
01101 {
01102 std::string tempValue;
01103 cameraProperty.propertyType = (ipa_CameraSensors::TYPE_UNSIGNED | ipa_CameraSensors::TYPE_CHARACTER);
01104 m_RangeCameraParameters.m_IntegrationTime.clear();
01105 m_RangeCameraParameters.m_IntegrationTime.seekg(0);
01106 m_RangeCameraParameters.m_IntegrationTime >> tempValue;
01107 cameraProperty.u_charData = (unsigned char)atoi(tempValue.c_str());
01108 }
01109
01110 if (SetProperty(&cameraProperty) & ipa_CameraSensors::RET_FAILED)
01111 {
01112 std::cout << "WARNING - Swissranger::SetParameters:" << std::endl;
01113 std::cout << "\t ... Could not set integration time" << std::endl;
01114 }
01115
01116
01117
01118
01119 cameraProperty.propertyID = ipa_CameraSensors::PROP_MODULATION_FREQUENCY;
01120 std::string sModulationFrequency = "";
01121 m_RangeCameraParameters.m_ModulationFrequency.clear();
01122 m_RangeCameraParameters.m_ModulationFrequency.seekg(0);
01123 m_RangeCameraParameters.m_ModulationFrequency >> sModulationFrequency;
01124 if (sModulationFrequency == "AUTO")
01125 {
01126 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01127 cameraProperty.specialValue = VALUE_AUTO;
01128 }
01129 else if (sModulationFrequency == "DEFAULT")
01130 {
01131 cameraProperty.propertyType = ipa_CameraSensors::TYPE_SPECIAL;
01132 cameraProperty.specialValue = VALUE_DEFAULT;
01133 }
01134 else
01135 {
01136 cameraProperty.propertyType = (ipa_CameraSensors::TYPE_STRING);
01137 m_RangeCameraParameters.m_ModulationFrequency.clear();
01138 m_RangeCameraParameters.m_ModulationFrequency.seekg(0);
01139 m_RangeCameraParameters.m_ModulationFrequency >> cameraProperty.stringData;
01140 }
01141
01142 if (SetProperty(&cameraProperty) & ipa_CameraSensors::RET_FAILED)
01143 {
01144 std::cout << "WARNING - Swissranger::SetParameters:" << std::endl;
01145 std::cout << "\t ... Could not set modulation frequency" << std::endl;
01146 }
01147
01148
01149
01150
01151 cameraProperty.propertyID = ipa_CameraSensors::PROP_ACQUIRE_MODE;
01152 cameraProperty.propertyType = ipa_CameraSensors::TYPE_INTEGER;
01153 m_RangeCameraParameters.m_AcquireMode.clear();
01154 m_RangeCameraParameters.m_AcquireMode.seekg(0);
01155 m_RangeCameraParameters.m_AcquireMode >> cameraProperty.integerData;
01156 if (SetProperty(&cameraProperty) & ipa_CameraSensors::RET_FAILED)
01157 {
01158 std::cout << "WARNING - Swissranger::SetParameters:" << std::endl;
01159 std::cout << "\t ... Could not set acquire mode" << std::endl;
01160 }
01161
01162 return RET_OK;
01163 }
01164
01165 unsigned long Swissranger::LoadParameters(const char* filename, int cameraIndex)
01166 {
01167
01168 boost::shared_ptr<TiXmlDocument> p_configXmlDocument (new TiXmlDocument( filename ));
01169
01170 if (!p_configXmlDocument->LoadFile())
01171 {
01172 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01173 std::cerr << "\t ... Error while loading xml configuration file \n";
01174 std::cerr << "\t ... (Check filename and syntax of the file):\n";
01175 std::cerr << "\t ... '" << filename << "'" << std::endl;
01176 return (RET_FAILED | RET_FAILED_OPEN_FILE);
01177 }
01178 std::cout << "INFO - Swissranger::LoadParameters:" << std::endl;
01179 std::cout << "\t ... Parsing xml configuration file:" << std::endl;
01180 std::cout << "\t ... '" << filename << "'" << std::endl;
01181
01182 std::string tempString;
01183 if ( p_configXmlDocument )
01184 {
01185
01186
01187
01188
01189
01190 TiXmlElement *p_xmlElement_Root = NULL;
01191 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
01192
01193 if ( p_xmlElement_Root )
01194 {
01195
01196
01197
01198
01199
01200 TiXmlElement *p_xmlElement_Root_SR31 = NULL;
01201 std::stringstream ss;
01202 ss << "Swissranger_" << cameraIndex;
01203 p_xmlElement_Root_SR31 = p_xmlElement_Root->FirstChildElement( ss.str() );
01204 if ( p_xmlElement_Root_SR31 )
01205 {
01206
01207
01208
01209
01210
01211 TiXmlElement* p_xmlElement_Child = NULL;
01212 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "Role" );
01213 if ( p_xmlElement_Child )
01214 {
01215
01216 if ( p_xmlElement_Child->QueryValueAttribute( "value", &tempString ) != TIXML_SUCCESS)
01217 {
01218 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01219 std::cerr << "\t ... Can't find attribute 'value' of tag 'Role'." << std::endl;
01220 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01221 }
01222
01223 if (tempString == "MASTER") m_RangeCameraParameters.m_CameraRole = MASTER;
01224 else if (tempString == "SLAVE") m_RangeCameraParameters.m_CameraRole = SLAVE;
01225 else
01226 {
01227 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01228 std::cerr << "\t ... Role " << tempString << " unspecified." << std::endl;
01229 return (RET_FAILED);
01230 }
01231
01232 }
01233 else
01234 {
01235 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01236 std::cerr << "\t ... Can't find tag 'Role'." << std::endl;
01237 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01238 }
01239
01240
01241
01242
01243
01244 p_xmlElement_Child = NULL;
01245 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "Interface" );
01246 std::string tempString;
01247 if ( p_xmlElement_Child )
01248 {
01249
01250 if ( p_xmlElement_Child->QueryValueAttribute( "value", &tempString ) != TIXML_SUCCESS)
01251 {
01252 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01253 std::cerr << "\t ... Can't find attribute 'value' of tag 'Interface'." << std::endl;
01254 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01255 }
01256 if (tempString == "USB")
01257 {
01258 m_RangeCameraParameters.m_Interface.str( " " );
01259 m_RangeCameraParameters.m_Interface.clear();
01260 m_RangeCameraParameters.m_Interface << tempString;
01261 }
01262 else if (tempString == "ETHERNET")
01263 {
01264 m_RangeCameraParameters.m_Interface.str( " " );
01265 m_RangeCameraParameters.m_Interface.clear();
01266 m_RangeCameraParameters.m_Interface << tempString;
01267
01268 if ( p_xmlElement_Child->QueryValueAttribute( "ip", &tempString ) != TIXML_SUCCESS)
01269 {
01270 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01271 std::cerr << "\t ... Can't find attribute 'ip' of tag 'Interface'." << std::endl;
01272 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01273 }
01274 m_RangeCameraParameters.m_IP.str( " " );
01275 m_RangeCameraParameters.m_IP.clear();
01276 m_RangeCameraParameters.m_IP << tempString;
01277 }
01278 else
01279 {
01280 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01281 std::cerr << "\t ... Interface " << tempString << " unspecified." << std::endl;
01282 return (RET_FAILED);
01283 }
01284 }
01285 else
01286 {
01287 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01288 std::cerr << "\t ... Can't find tag 'Interface'." << std::endl;
01289 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01290 }
01291
01292
01293
01294
01295
01296 p_xmlElement_Child = NULL;
01297 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "AmplitudeThreshold" );
01298 if ( p_xmlElement_Child )
01299 {
01300
01301 if ( p_xmlElement_Child->QueryValueAttribute( "value", &tempString) != TIXML_SUCCESS)
01302 {
01303 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01304 std::cerr << "\t ... Can't find attribute 'value' of tag 'AmplitudeThreshold'." << std::endl;
01305 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01306 }
01307 else
01308 {
01309 m_RangeCameraParameters.m_AmplitudeThreshold.str( " " );
01310 m_RangeCameraParameters.m_AmplitudeThreshold.clear();
01311 m_RangeCameraParameters.m_AmplitudeThreshold << tempString;
01312 }
01313 }
01314 else
01315 {
01316 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01317 std::cerr << "\t ... Can't find tag 'AmplitudeThreshold'." << std::endl;
01318 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01319 }
01320
01321
01322
01323
01324
01325 p_xmlElement_Child = NULL;
01326 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "IntegrationTime" );
01327 if ( p_xmlElement_Child )
01328 {
01329
01330 if ( p_xmlElement_Child->QueryValueAttribute( "value", &tempString) != TIXML_SUCCESS)
01331 {
01332 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01333 std::cerr << "\t ... Can't find attribute 'value' of tag 'IntegrationTime'." << std::endl;
01334 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01335 }
01336 else
01337 {
01338 m_RangeCameraParameters.m_IntegrationTime.str( " " );
01339 m_RangeCameraParameters.m_IntegrationTime.clear();
01340 m_RangeCameraParameters.m_IntegrationTime << tempString;
01341 }
01342 }
01343 else
01344 {
01345 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01346 std::cerr << "\t ... Can't find tag 'IntegrationTime'." << std::endl;
01347 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01348 }
01349
01350
01351
01352
01353
01354 p_xmlElement_Child = NULL;
01355 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "Modulation" );
01356 if ( p_xmlElement_Child )
01357 {
01358
01359 if ( p_xmlElement_Child->QueryValueAttribute( "frequency", &tempString) != TIXML_SUCCESS)
01360 {
01361 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01362 std::cerr << "\t ... Can't find attribute 'frequency' of tag 'Modulation'." << std::endl;
01363 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01364 }
01365 else
01366 {
01367 m_RangeCameraParameters.m_ModulationFrequency.str( " " );
01368 m_RangeCameraParameters.m_ModulationFrequency.clear();
01369 m_RangeCameraParameters.m_ModulationFrequency << tempString;
01370 }
01371 }
01372 else
01373 {
01374 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01375 std::cerr << "\t ... Can't find tag 'Modulation'." << std::endl;
01376 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01377 }
01378
01379
01380
01381
01382
01383 p_xmlElement_Child = NULL;
01384 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "AcquireMode" );
01385 if ( p_xmlElement_Child )
01386 {
01387 int acquireMode = 0;
01388
01389 if ( p_xmlElement_Child->QueryValueAttribute( "AM_COR_FIX_PTRN", &tempString) != TIXML_SUCCESS)
01390 {
01391 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01392 std::cerr << "\t ... Can't find attribute 'AM_COR_FIX_PTRN' of tag 'AcquireMode'." << std::endl;
01393 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01394 }
01395 else
01396 {
01397 if (tempString == "ON") acquireMode |= AM_COR_FIX_PTRN;
01398 }
01399
01400 if ( p_xmlElement_Child->QueryValueAttribute( "AM_MEDIAN", &tempString) != TIXML_SUCCESS)
01401 {
01402 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01403 std::cerr << "\t ... Can't find attribute 'AM_MEDIAN' of tag 'AcquireMode'." << std::endl;
01404 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01405 }
01406 else
01407 {
01408 if (tempString == "ON") acquireMode |= AM_MEDIAN;
01409 }
01410
01411 if ( p_xmlElement_Child->QueryValueAttribute( "AM_TOGGLE_FRQ", &tempString) != TIXML_SUCCESS)
01412 {
01413 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01414 std::cerr << "\t ... Can't find attribute 'AM_TOGGLE_FRQ' of tag 'AcquireMode'." << std::endl;
01415 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01416 }
01417 else
01418 {
01419 if (tempString == "ON") acquireMode |= AM_TOGGLE_FRQ;
01420 }
01421
01422 if ( p_xmlElement_Child->QueryValueAttribute( "AM_CONV_GRAY", &tempString) != TIXML_SUCCESS)
01423 {
01424 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01425 std::cerr << "\t ... Can't find attribute 'AM_CONV_GRAY' of tag 'AcquireMode'." << std::endl;
01426 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01427 }
01428 else
01429 {
01430 if (tempString == "ON") acquireMode |= AM_CONV_GRAY;
01431 }
01432
01433 if ( p_xmlElement_Child->QueryValueAttribute( "AM_SW_ANF", &tempString) != TIXML_SUCCESS)
01434 {
01435 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01436 std::cerr << "\t ... Can't find attribute 'AM_SW_ANF' of tag 'AcquireMode'." << std::endl;
01437 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01438 }
01439 else
01440 {
01441 if (tempString == "ON") acquireMode |= AM_SW_ANF;
01442 }
01443
01444 if ( p_xmlElement_Child->QueryValueAttribute( "AM_SR3K_2TAP_PROC", &tempString) != TIXML_SUCCESS)
01445 {
01446 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01447 std::cerr << "\t ... Can't find attribute 'AM_SR3K_2TAP_PROC' of tag 'AcquireMode'." << std::endl;
01448 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01449 }
01450 else
01451 {
01452
01453 }
01454
01455 if ( p_xmlElement_Child->QueryValueAttribute( "AM_SHORT_RANGE", &tempString) != TIXML_SUCCESS)
01456 {
01457 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01458 std::cerr << "\t ... Can't find attribute 'AM_SHORT_RANGE' of tag 'AcquireMode'." << std::endl;
01459 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01460 }
01461 else
01462 {
01463
01464 }
01465
01466 if ( p_xmlElement_Child->QueryValueAttribute( "AM_CONF_MAP", &tempString) != TIXML_SUCCESS)
01467 {
01468 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01469 std::cerr << "\t ... Can't find attribute 'AM_CONF_MAP' of tag 'AcquireMode'." << std::endl;
01470 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01471 }
01472 else
01473 {
01474 if (tempString == "ON") acquireMode |= AM_CONF_MAP;
01475 }
01476
01477 if ( p_xmlElement_Child->QueryValueAttribute( "AM_HW_TRIGGER", &tempString) != TIXML_SUCCESS)
01478 {
01479 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01480 std::cerr << "\t ... Can't find attribute 'AM_HW_TRIGGER' of tag 'AcquireMode'." << std::endl;
01481 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01482 }
01483 else
01484 {
01485 if (tempString == "ON") acquireMode |= AM_HW_TRIGGER;
01486 }
01487
01488 if ( p_xmlElement_Child->QueryValueAttribute( "AM_SW_TRIGGER", &tempString) != TIXML_SUCCESS)
01489 {
01490 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01491 std::cerr << "\t ... Can't find attribute 'AM_SW_TRIGGER' of tag 'AcquireMode'." << std::endl;
01492 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01493 }
01494 else
01495 {
01496 if (tempString == "ON") acquireMode |= AM_SW_TRIGGER;
01497 }
01498
01499 if ( p_xmlElement_Child->QueryValueAttribute( "AM_DENOISE_ANF", &tempString) != TIXML_SUCCESS)
01500 {
01501 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01502 std::cerr << "\t ... Can't find attribute 'AM_DENOISE_ANF' of tag 'AcquireMode'." << std::endl;
01503 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01504 }
01505 else
01506 {
01507 if (tempString == "ON") acquireMode |= AM_DENOISE_ANF;
01508 }
01509
01510 if ( p_xmlElement_Child->QueryValueAttribute( "AM_MEDIANCROSS", &tempString) != TIXML_SUCCESS)
01511 {
01512 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01513 std::cerr << "\t ... Can't find attribute 'AM_MEDIANCROSS' of tag 'AcquireMode'." << std::endl;
01514 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01515 }
01516 else
01517 {
01518 if (tempString == "ON") acquireMode |= AM_MEDIANCROSS;
01519 }
01520
01521 m_RangeCameraParameters.m_AcquireMode.str( " " );
01522 m_RangeCameraParameters.m_AcquireMode.clear();
01523 m_RangeCameraParameters.m_AcquireMode << acquireMode;
01524 }
01525 else
01526 {
01527 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01528 std::cerr << "\t ... Can't find tag 'AcquireMode'." << std::endl;
01529 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01530 }
01531
01532
01533
01534
01535
01536 p_xmlElement_Child = NULL;
01537 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "CalibrationMethod" );
01538 if ( p_xmlElement_Child )
01539 {
01540
01541 if ( p_xmlElement_Child->QueryValueAttribute( "name", &tempString ) != TIXML_SUCCESS)
01542 {
01543 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01544 std::cerr << "\t ... Can't find attribute 'name' of tag 'CalibrationMethod'." << std::endl;
01545 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01546 }
01547 if (tempString == "MATLAB") m_CalibrationMethod = MATLAB;
01548 else if (tempString == "MATLAB_NO_Z") m_CalibrationMethod = MATLAB_NO_Z;
01549 else if (tempString == "NATIVE") m_CalibrationMethod = NATIVE;
01550 else
01551 {
01552 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01553 std::cerr << "\t ... Calibration mode " << tempString << " unspecified." << std::endl;
01554 return (RET_FAILED);
01555 }
01556 }
01557 else
01558 {
01559 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01560 std::cerr << "\t ... Can't find tag 'CalibrationMethod'." << std::endl;
01561 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01562 }
01563
01564 }
01565
01566
01567
01568 else
01569 {
01570 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01571 std::cerr << "\t ... Can't find tag '" << ss.str() << "'" << std::endl;
01572 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01573 }
01574 }
01575
01576
01577
01578
01579 else
01580 {
01581 std::cerr << "ERROR - Swissranger::LoadParameters:" << std::endl;
01582 std::cerr << "\t ... Can't find tag 'LibCameraSensors'." << std::endl;
01583 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01584 }
01585 }
01586
01587
01588 std::cout << "INFO - Swissranger::LoadParameters:" << std::endl;
01589 std::cout << "\t ... Parsing xml calibration file: Done.\n";
01590
01591
01592
01593 return RET_OK;
01594 }