cob_camera_sensors: cob_camera_sensors: Swissranger.cpp Source File

00001 /****************************************************************
00002 *
00003 * Copyright (c) 2010
00004 *
00005 * Fraunhofer Institute for Manufacturing Engineering
00006 * and Automation (IPA)
00007 *
00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009 *
00010 * Project name: care-o-bot
00011 * ROS stack name: cob_driver
00012 * ROS package name: cob_camera_sensors
00013 * Description:
00014 *
00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016 *
00017 * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
00018 * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
00019 *
00020 * Date of creation: Mai 2008
00021 * ToDo:
00022 *
00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024 *
00025 * Redistribution and use in source and binary forms, with or without
00026 * modification, are permitted provided that the following conditions are met:
00027 *
00028 * * Redistributions of source code must retain the above copyright
00029 * notice, this list of conditions and the following disclaimer.
00030 * * Redistributions in binary form must reproduce the above copyright
00031 * notice, this list of conditions and the following disclaimer in the
00032 * documentation and/or other materials provided with the distribution.
00033 * * Neither the name of the Fraunhofer Institute for Manufacturing
00034 * Engineering and Automation (IPA) nor the names of its
00035 * contributors may be used to endorse or promote products derived from
00036 * this software without specific prior written permission.
00037 *
00038 * This program is free software: you can redistribute it and/or modify
00039 * it under the terms of the GNU Lesser General Public License LGPL as
00040 * published by the Free Software Foundation, either version 3 of the
00041 * License, or (at your option) any later version.
00042 *
00043 * This program is distributed in the hope that it will be useful,
00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00046 * GNU Lesser General Public License LGPL for more details.
00047 *
00048 * You should have received a copy of the GNU Lesser General Public
00049 * License LGPL along with this program.
00050 * If not, see <http://www.gnu.org/licenses/>.
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: // Redirects all output to console
00097                 {
00098                         if (param==MC_ETH)
00099                         {
00100                                 // Do nothing
00101                                 return 0;
00102                         }
00103                         else
00104                         {
00105                                 return SR_GetDefaultCallback()(0,msg,param,data);
00106                         }
00107                         break;
00108                 }
00109                 default:
00110                 {
00111                         // Default handling
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         // Load SR parameters from xml-file
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         // Set callback function, to catch annoying ethernet messages
00139         SR_SetCallback(LibMesaCallback);
00140 
00141         if (m_CalibrationMethod == MATLAB)
00142         {
00143                 // Load z-calibration files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
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                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00249                 }
00250                 else
00251                 {
00252                         m_CoeffsA6 = c_mat;
00253                         cvReleaseMat(&c_mat);
00254                 }
00255         }
00256         
00257         // set init flag
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(); // Clear flags
00279         m_RangeCameraParameters.m_Interface.seekg(0); // Set Pointer to position 0 within stringstream
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(); // Clear flags
00296                 m_RangeCameraParameters.m_IP.seekg(0); // Set Pointer to position 0 within stringstream
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         //char DevStr[1024];
00315         //SR_GetDeviceString(m_SRCam, DevStr, 1024);
00316         //std::cout << "Swissranger::Open(): INFO" << std::endl;
00317         //std::cout << "\t ... " << DevStr << std::endl;
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                                         // Void
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                                         // Void
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                                         // Void
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                                 // MF_40MHz, SR3k: maximal range 3.75m
00483                 // MF_30MHz, SR3k, SR4k: maximal range 5m
00484                 // MF_21MHz, SR3k: maximal range 7.14m
00485                 // MF_20MHz, SR3k: maximal range 7.5m
00486                 // MF_19MHz, SR3k: maximal range 7.89m
00487                 // MF_60MHz, SR4k: maximal range 2.5m 
00488                 // MF_15MHz, SR4k: maximal range 10m
00489                 // MF_10MHz, SR4k: maximal range 15m
00490                 // MF_29MHz, SR4k: maximal range 5.17m
00491                 // MF_31MHz
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 // Wrapper for IplImage retrival from AcquireImage
00659 // Images have to be initialized prior to calling this function
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 // Enables faster image retrival than AcquireImage
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 // Get data from camera
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         //unsigned int c = SR_GetIntegrationTime(m_SRCam);
00723         //unsigned short a = SR_GetAmplitudeThreshold(m_SRCam);
00724         //std::cout << "\t ... Integration time is '" << c << "'" << std::endl;
00725         //std::cout << "\t ... Amplitude threshold is '" << a << "'" << std::endl;
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 // Range image (distorted or undistorted)
00758         if (rangeImageData)
00759         {
00760                 int imageStep = -1;
00761                 float* f_ptr = 0;
00762                 
00763                 // put data in corresponding IPLImage structures
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         } // End if (rangeImage)
00785 
00787 // Intensity/Amplitude image
00788 // ATTENTION: SR provides only amplitude information
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 // Cartesian image (always undistorted)
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                                 // Calculate calibrated z values (in meter) based on 6 degree polynomial approximation
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                                 /*IplImage dummy;
00856                                 IplImage *z = cvGetImage(distortedData, &dummy);
00857                                 IplImage *image = cvCreateImage(cvGetSize(z), IPL_DEPTH_8U, 3);
00858                                 ipa_Utils::ConvertToShowImage(z, image, 1);
00859                                 cvNamedWindow("Z");
00860                                 cvShowImage("Z", image);
00861                                 cvWaitKey();
00862                                 */
00863 
00864                                 // Undistort
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                                 /*IplImage dummy;
00870                                 IplImage* z = cvGetImage(undistortedData, &dummy);
00871                                 IplImage* image = cvCreateImage(cvGetSize(z), IPL_DEPTH_8U, 3);
00872                                 ipa_utils::ConvertToShowImage(z, image, 1);
00873                                 cvNamedWindow("Z");
00874                                 cvShowImage("Z", image);
00875                                 cvWaitKey();*/
00876 
00877                                 // Calculate X and Y based on instrinsic rotation and translation
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                         // Calculate calibrated z values (in meter) based on 6 degree polynomial approximation
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                         // Undistort
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                         // Calculate X and Y based on instrinsic rotation and translation
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 // Return value is in m
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 // u and v are assumed to be distorted coordinates
00996 unsigned long Swissranger::GetCalibratedXYMatlab(int u, int v, float z, float& x, float& y)
00997 {
00998         // Conversion form m to mm
00999         z *= 1000;
01000 
01001         // Use intrinsic camera parameters
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         // Fundamental equation: u = (fx*x)/z + cx
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         // Fundamental equation: v = (fy*y)/z + cy
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         // Conversion from mm to m
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         // make sure, that m_X, m_Y and m_Z have been initialized by Acquire image
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 // Set amplitude threshold
01052 // -----------------------------------------------------------------
01053         cameraProperty.propertyID = ipa_CameraSensors::PROP_AMPLITUDE_THRESHOLD;
01054         std::string sAmplitudeThreshold = "";
01055         m_RangeCameraParameters.m_AmplitudeThreshold.clear(); // Clear flags
01056         m_RangeCameraParameters.m_AmplitudeThreshold.seekg(0); // Set Pointer to position 0 within stringstream
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(); // Clear flags
01072                 m_RangeCameraParameters.m_AmplitudeThreshold.seekg(0); // Set Pointer to position 0 within stringstream
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 // Set integration time
01084 // -----------------------------------------------------------------
01085         cameraProperty.propertyID = ipa_CameraSensors::PROP_INTEGRATION_TIME;
01086         std::string sIntegrationTime = "";
01087         m_RangeCameraParameters.m_IntegrationTime.clear(); // Clear flags
01088         m_RangeCameraParameters.m_IntegrationTime.seekg(0); // Set Pointer to position 0 within stringstream
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(); // Clear flags
01105                 m_RangeCameraParameters.m_IntegrationTime.seekg(0); // Set Pointer to position 0 within stringstream
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 // Set modulation frequency
01118 // -----------------------------------------------------------------
01119         cameraProperty.propertyID = ipa_CameraSensors::PROP_MODULATION_FREQUENCY;
01120         std::string sModulationFrequency = "";
01121         m_RangeCameraParameters.m_ModulationFrequency.clear(); // Clear flags
01122         m_RangeCameraParameters.m_ModulationFrequency.seekg(0); // Set Pointer to position 0 within stringstream
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(); // Clear flags
01138                 m_RangeCameraParameters.m_ModulationFrequency.seekg(0); // Set Pointer to position 0 within stringstream
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 // Set acquire mode
01150 // -----------------------------------------------------------------
01151         cameraProperty.propertyID = ipa_CameraSensors::PROP_ACQUIRE_MODE;
01152         cameraProperty.propertyType = ipa_CameraSensors::TYPE_INTEGER;
01153         m_RangeCameraParameters.m_AcquireMode.clear(); // Set Pointer to position 0 within stringstream
01154         m_RangeCameraParameters.m_AcquireMode.seekg(0); // Set Pointer to position 0 within stringstream
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         // Load SwissRanger parameters.
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 //      BEGIN LibCameraSensors
01188 //************************************************************************************
01189                 // Tag element "LibCameraSensors" of Xml Inifile                
01190                 TiXmlElement *p_xmlElement_Root = NULL;
01191                 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
01192 
01193                 if ( p_xmlElement_Root )
01194                 {
01195 
01196 //************************************************************************************
01197 //      BEGIN LibCameraSensors->Swissranger
01198 //************************************************************************************
01199                         // Tag element "Swissranger3000" of Xml Inifile         
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 //      BEGIN LibCameraSensors->Swissranger->Role
01209 //************************************************************************************
01210                                 // Subtag element "Role" of Xml Inifile
01211                                 TiXmlElement* p_xmlElement_Child = NULL;
01212                                 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "Role" );
01213                                 if ( p_xmlElement_Child )
01214                                 {
01215                                         // read and save value of attribute
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 //      BEGIN LibCameraSensors->Swissranger->Interface
01242 //************************************************************************************
01243                                 // Subtag element "OperationMode" of Xml Inifile
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                                         // read and save value of attribute
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( " " ); // Clear stringstream
01259                                                 m_RangeCameraParameters.m_Interface.clear();            // Reset flags
01260                                                 m_RangeCameraParameters.m_Interface << tempString;
01261                                         }
01262                                         else if (tempString == "ETHERNET")
01263                                         {
01264                                                 m_RangeCameraParameters.m_Interface.str( " " ); // Clear stringstream
01265                                                 m_RangeCameraParameters.m_Interface.clear();            // Reset flags
01266                                                 m_RangeCameraParameters.m_Interface << tempString;
01267                                                 // read and save value of attribute
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( " " );        // Clear stringstream
01275                                                 m_RangeCameraParameters.m_IP.clear();           // Reset flags
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 //      BEGIN LibCameraSensors->Swissranger->AmplitudeThreshold
01294 //************************************************************************************
01295                                 // Subtag element "IntegrationTime" of Xml Inifile
01296                                 p_xmlElement_Child = NULL;
01297                                 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "AmplitudeThreshold" );
01298                                 if ( p_xmlElement_Child )
01299                                 {
01300                                         // read and save value of attribute
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( " " );        // Clear stringstream
01310                                                 m_RangeCameraParameters.m_AmplitudeThreshold.clear();           // Reset flags
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 //      BEGIN LibCameraSensors->Swissranger->IntegrationTime
01323 //************************************************************************************
01324                                 // Subtag element "IntegrationTime" of Xml Inifile
01325                                 p_xmlElement_Child = NULL;
01326                                 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "IntegrationTime" );
01327                                 if ( p_xmlElement_Child )
01328                                 {
01329                                         // read and save value of attribute
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( " " );   // Clear stringstream
01339                                                 m_RangeCameraParameters.m_IntegrationTime.clear();              // Reset flags
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 //      BEGIN LibCameraSensors->Swissranger->Modulation
01352 //************************************************************************************
01353                                 // Subtag element "IntegrationTime" of Xml Inifile
01354                                 p_xmlElement_Child = NULL;
01355                                 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "Modulation" );
01356                                 if ( p_xmlElement_Child )
01357                                 {
01358                                         // read and save value of attribute
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( " " );       // Clear stringstream
01368                                                 m_RangeCameraParameters.m_ModulationFrequency.clear();          // Reset flags
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 //      BEGIN LibCameraSensors->Swissranger->AcquireMode
01381 //************************************************************************************
01382                                 // Subtag element "IntegrationTime" of Xml Inifile
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                                         // read and save value of attribute
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                                                 //if (tempString == "ON") acquireMode |= AM_RESERVED0;
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                                                 //if (tempString == "ON") acquireMode |= AM_RESERVED1;
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( " " );       // Clear stringstream
01522                                         m_RangeCameraParameters.m_AcquireMode.clear();          // Reset flags
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 //      BEGIN LibCameraSensors->Swissranger->CalibrationMethod
01534 //************************************************************************************
01535                                 // Subtag element "OperationMode" of Xml Inifile
01536                                 p_xmlElement_Child = NULL;
01537                                 p_xmlElement_Child = p_xmlElement_Root_SR31->FirstChildElement( "CalibrationMethod" );
01538                                 if ( p_xmlElement_Child )
01539                                 {
01540                                         // read and save value of attribute
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 //      END LibCameraSensors->Swissranger
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 //      END LibCameraSensors
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 }