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