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

cob_camera_sensors
Author(s): Jan Fischer , Richard Bormann
autogenerated on Sat Jun 8 2019 21:02:02