VirtualRangeCam.cpp
Go to the documentation of this 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 #include "../include/cob_camera_sensors/StdAfx.h"
00055 
00056 #ifdef __LINUX__
00057 #include "cob_camera_sensors/VirtualRangeCam.h"
00058 #include "cob_vision_utils/VisionUtils.h"
00059 #include "tinyxml.h"
00060 #else
00061 #include "cob_driver/cob_camera_sensors/common/include/cob_camera_sensors/VirtualRangeCam.h"
00062 #include "cob_common/cob_vision_utils/common/include/cob_vision_utils/VisionUtils.h"
00063 #include "cob_vision/windows/src/extern/TinyXml/tinyxml.h"
00064 #endif
00065 
00066 #include <opencv/highgui.h>
00067 
00068 namespace fs = boost::filesystem;
00069 using namespace ipa_CameraSensors;
00070 
00071 __DLL_LIBCAMERASENSORS__ AbstractRangeImagingSensorPtr ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam()
00072 {
00073         return AbstractRangeImagingSensorPtr(new VirtualRangeCam());
00074 }
00075 
00076 VirtualRangeCam::VirtualRangeCam()
00077 {
00078         m_initialized = false;
00079         m_open = false;
00080 
00081         m_BufferSize = 1;
00082 
00083         m_ImageCounter = 0;
00084 }
00085 
00086 
00087 VirtualRangeCam::~VirtualRangeCam()
00088 {
00089         if (isOpen())
00090         {
00091                 Close();
00092         }
00093 }
00094 
00095 unsigned long VirtualRangeCam::Init(std::string directory, int cameraIndex)
00096 {
00097         if (isInitialized())
00098         {
00099                 return (RET_OK | RET_CAMERA_ALREADY_INITIALIZED);
00100         }
00101 
00102         m_CameraType = ipa_CameraSensors::CAM_VIRTUALRANGE;
00103 
00104         // It is important to put this before LoadParameters
00105         m_CameraDataDirectory = directory;
00106 
00107         // Load SR parameters from xml-file
00108         if (LoadParameters((directory + "cameraSensorsIni.xml").c_str(), cameraIndex) & RET_FAILED)
00109         {
00110                 std::cerr << "ERROR - VirtualRangeCam::Init:" << std::endl;
00111                 std::cerr << "\t ... Parsing xml configuration file failed" << std::endl;
00112                 return (RET_FAILED | RET_INIT_CAMERA_FAILED);
00113         }
00114 
00115         m_CoeffsInitialized = true;
00116         if (m_CalibrationMethod == MATLAB)
00117         {
00118                 // Load z-calibration files
00119                 std::string filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA0.xml";
00120                 CvMat* c_mat = (CvMat*)cvLoad(filename.c_str()); 
00121                 if (! c_mat)
00122                 {
00123                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00124                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA0.txt" << "." << std::endl;
00125                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00126                         m_CoeffsInitialized = false;
00127                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00128                 }
00129                 else
00130                 {
00131                         m_CoeffsA0 = c_mat;
00132                         cvReleaseMat(&c_mat);
00133                 }
00134 
00135                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA1.xml";
00136                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00137                 if (! c_mat)
00138                 {
00139                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00140                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA1.txt" << "." << std::endl;
00141                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00142                         m_CoeffsInitialized = false;
00143                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00144                 }
00145                 else
00146                 {
00147                         m_CoeffsA1 = c_mat;
00148                         cvReleaseMat(&c_mat);
00149                 }
00150 
00151                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA2.xml";
00152                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00153                 if (! c_mat)
00154                 {
00155                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00156                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA2.txt" << "." << std::endl;
00157                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00158                         m_CoeffsInitialized = false;
00159                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00160                 }
00161                 else
00162                 {
00163                         m_CoeffsA2 = c_mat;
00164                         cvReleaseMat(&c_mat);
00165                 }
00166 
00167                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA3.xml";
00168                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00169                 if (! c_mat)
00170                 {
00171                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00172                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA3.txt" << "." << std::endl;
00173                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00174                         m_CoeffsInitialized = false;
00175                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00176                 }
00177                 else
00178                 {
00179                         m_CoeffsA3 = c_mat;
00180                         cvReleaseMat(&c_mat);
00181                 }
00182 
00183                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA4.xml";
00184                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00185                 if (! c_mat)
00186                 {
00187                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00188                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA4.txt" << "." << std::endl;
00189                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00190                         m_CoeffsInitialized = false;
00191                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00192                 }
00193                 else
00194                 {
00195                         m_CoeffsA4 = c_mat;
00196                         cvReleaseMat(&c_mat);
00197                 }
00198 
00199                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA5.xml";
00200                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00201                 if (! c_mat)
00202                 {
00203                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00204                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA5.txt" << "." << std::endl;
00205                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00206                         m_CoeffsInitialized = false;
00207                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00208                 }
00209                 else
00210                 {
00211                         m_CoeffsA5 = c_mat;
00212                         cvReleaseMat(&c_mat);
00213                 }
00214 
00215                 filename = directory + "MatlabCalibrationData/PMD/ZCoeffsA6.xml";
00216                 c_mat = (CvMat*)cvLoad(filename.c_str()); 
00217                 if (! c_mat)
00218                 {
00219                         std::cerr << "ERROR - PMDCamCube::LoadParameters:" << std::endl;
00220                         std::cerr << "\t ... Error while loading " << directory + "MatlabCalibrationData/ZcoeffsA6.txt" << "." << std::endl;
00221                         std::cerr << "\t ... Data is necessary for z-calibration of swissranger camera" << std::endl;
00222                         m_CoeffsInitialized = false;
00223                         // no RET_FAILED, as we might want to calibrate the camera to create these files
00224                 }
00225                 else
00226                 {
00227                         m_CoeffsA6 = c_mat;
00228                         cvReleaseMat(&c_mat);
00229                 }
00230         }
00231 
00232         m_CameraIndex = cameraIndex;
00233 
00234         // set init flag
00235         m_initialized = true;
00236 
00237         return RET_OK;
00238 }
00239 
00240 
00241 inline void VirtualRangeCam::UpdateImageDimensionsOnFirstImage(std::string filename, std::string ext)
00242 {
00243         if (m_ImageWidth == -1 || m_ImageHeight == -1)
00244         {
00245                 if (ext != ".bin")
00246                 {
00247                         IplImage* image = (IplImage*) cvLoad(filename.c_str(), 0);
00248                         m_ImageWidth = image->width;
00249                         m_ImageHeight = image->height;
00250                         cvReleaseImage(&image);
00251                 }
00252                 else
00253                 {
00254                         cv::Mat mat;
00255                         ipa_Utils::LoadMat(mat, filename);
00256                         m_ImageHeight = mat.rows;
00257                         m_ImageWidth = mat.cols;
00258                 }
00259         }
00260 }
00261 
00262 
00263 inline void VirtualRangeCam::FindSourceImageFormat(std::map<std::string, int>::iterator& itCounter, std::string& ext)
00264 {
00265         if (itCounter->second > 0)
00266         {
00267                 if (ext == "") ext = itCounter->first;
00268                 else
00269                 {
00270                         std::cerr << "ERROR - VirtualRangeCam::Open:\n";
00271                         std::cerr << "\t ... The provided path contains data in mixed formats (e.g. .xml and .bin).\n";
00272                         assert(false);
00273                 }
00274         }
00275 }
00276 
00277 
00278 unsigned long VirtualRangeCam::Open()
00279 {
00280         if (!isInitialized())
00281         {
00282                 return (RET_FAILED | RET_CAMERA_NOT_INITIALIZED);
00283         }
00284 
00285         if (isOpen())
00286         {
00287                 return (RET_OK | RET_CAMERA_ALREADY_OPEN);
00288         }
00289 
00290         // Convert camera ID to string
00291         std::stringstream ss;
00292         std::string sCameraIndex;
00293         ss << m_CameraIndex;
00294         ss >> sCameraIndex;
00295 
00296         m_ImageWidth = -1;
00297         m_ImageHeight = -1;
00298 
00299         // Create absolute filename and check if directory exists
00300         fs::path absoluteDirectoryName( m_CameraDataDirectory );
00301         if ( !fs::exists( absoluteDirectoryName ) )
00302         {
00303                 std::cerr << "ERROR - VirtualRangeCam::Open:" << std::endl;
00304                 std::cerr << "\t ... Path '" << absoluteDirectoryName.file_string() << "' not found" << std::endl;
00305                 return (ipa_CameraSensors::RET_FAILED | ipa_CameraSensors::RET_FAILED_OPEN_FILE);
00306         }
00307 
00308         std::vector<std::string> extensionList;
00309         extensionList.push_back(".xml"); extensionList.push_back(".bin"); extensionList.push_back(".png"); extensionList.push_back(".jpg"); extensionList.push_back(".bmp");
00310         std::map<std::string, int> amplitudeImageCounter;       // first index is the extension (.xml, .bin), second is the number of such images found
00311         std::map<std::string, int> intensityImageCounter;       // first index is the extension (.xml, .bin, .png, .jpg, .bmp), second is the number of such images found
00312         std::map<std::string, int> coordinateImageCounter;      // first index is the extension (.xml, .bin), second is the number of such images found
00313         std::map<std::string, int> rangeImageCounter;           // first index is the extension (.xml, .bin), second is the number of such images found
00314         std::map<std::string, std::vector<std::string> > amplitudeImageFileNames;       // first index is the extension (.xml, .bin), second is the vector of file names
00315         std::map<std::string, std::vector<std::string> > intensityImageFileNames;       // first index is the extension (.xml, .bin, .png, .jpg, .bmp), second is the vector of file names
00316         std::map<std::string, std::vector<std::string> > coordinateImageFileNames;      // first index is the extension (.xml, .bin), second is the vector of file names
00317         std::map<std::string, std::vector<std::string> > rangeImageFileNames;           // first index is the extension (.xml, .bin), second is the vector of file names
00318         // Extract all image filenames from the directory
00319         if ( fs::exists( absoluteDirectoryName ) )
00320         {
00321                 std::cout << "INFO - VirtualRangeCam::Open   :" << std::endl;
00322                 std::cout << "\t ... Parsing directory '" << absoluteDirectoryName.directory_string() << "'" << std::endl;
00323                 
00324                 fs::directory_iterator end_iter;
00325                 for ( fs::directory_iterator dir_itr( absoluteDirectoryName ); dir_itr != end_iter; ++dir_itr )
00326                 {
00327                         try
00328                         {
00329                                 if (fs::is_regular_file(dir_itr->status()))
00330                                 {
00331                                         std::string filename = dir_itr->path().string();
00332 
00333                                         // intensity image formats
00334                                         for (unsigned int extIndex=0; extIndex<extensionList.size(); extIndex++)
00335                                         {
00336                                                 std::string ext = extensionList[extIndex];
00337                                                 std::string format = "32F1_";
00338                                                 if (extIndex>=2) format = "8U3_";
00339                                                 if ((dir_itr->path().extension() == ext) && filename.find( "RangeCamIntensity_" + format + sCameraIndex, 0 ) != std::string::npos)
00340                                                 {
00341                                                         (intensityImageCounter.find(ext) == intensityImageCounter.end()) ? intensityImageCounter[ext] = 1 : intensityImageCounter[ext]++;
00342                                                         //std::cout << "VirtualRangeCam::Open(): Reading '" << filename << "\n";
00343                                                         intensityImageFileNames[ext].push_back(filename);
00344                                                         UpdateImageDimensionsOnFirstImage(filename, ext);
00345                                                 }
00346                                         }
00347 
00348                                         // amplitude image formats
00349                                         for (unsigned int extIndex=0; extIndex<2; extIndex++)
00350                                         {
00351                                                 std::string ext = extensionList[extIndex];
00352                                                 if ((dir_itr->path().extension() == ext) && filename.find( "RangeCamAmplitude_32F1_" + sCameraIndex, 0 ) != std::string::npos)
00353                                                 {
00354                                                         (amplitudeImageCounter.find(ext) == amplitudeImageCounter.end()) ? amplitudeImageCounter[ext] = 1 : amplitudeImageCounter[ext]++;
00355                                                         //std::cout << "VirtualRangeCam::Open(): Reading '" << filename << "\n";
00356                                                         amplitudeImageFileNames[ext].push_back(filename);
00357                                                         UpdateImageDimensionsOnFirstImage(filename, ext);
00358                                                 }
00359                                         }
00360 
00361                                         // coordinate image formats
00362                                         for (unsigned int extIndex=0; extIndex<2; extIndex++)
00363                                         {
00364                                                 std::string ext = extensionList[extIndex];
00365                                                 if ((dir_itr->path().extension() == ext) && filename.find( "RangeCamCoordinate_32F3_" + sCameraIndex, 0 ) != std::string::npos)
00366                                                 {
00367                                                         (coordinateImageCounter.find(ext) == coordinateImageCounter.end()) ? coordinateImageCounter[ext] = 1 : coordinateImageCounter[ext]++;
00368                                                         coordinateImageFileNames[ext].push_back(filename);
00369                                                         UpdateImageDimensionsOnFirstImage(filename, ext);
00370                                                         //std::cout << "VirtualRangeCam::Open(): Reading '" << filename << "\n";
00371                                                 }
00372                                         }
00373 
00374                                         // range image formats
00375                                         for (unsigned int extIndex=0; extIndex<2; extIndex++)
00376                                         {
00377                                                 std::string ext = extensionList[extIndex];
00378                                                 if ((dir_itr->path().extension() == ext) && filename.find( "RangeCamRange_32F1_" + sCameraIndex, 0 ) != std::string::npos)
00379                                                 {
00380                                                         (rangeImageCounter.find(ext) == rangeImageCounter.end()) ? rangeImageCounter[ext] = 1 : rangeImageCounter[ext]++;
00381                                                         //std::cout << "VirtualRangeCam::Open(): Reading '" << filename << "\n";
00382                                                         rangeImageFileNames[ext].push_back(filename);
00383                                                         UpdateImageDimensionsOnFirstImage(filename, ext);
00384                                                 }
00385                                         }
00386                                 }
00387                         }
00388                         catch ( const std::exception &ex )
00389                         {
00390                                 std::cout << "WARNING - VirtualRangeCam::Open:" << std::endl;
00391                                 std::cout << "\t ... Exception catch of '" << ex.what() << "'" << std::endl;
00392                         }
00393                 }
00394                 // intensity
00395                 std::map<std::string, int>::iterator itCounter;
00396                 std::string extInt = "";
00397                 for (itCounter = intensityImageCounter.begin(); itCounter != intensityImageCounter.end(); itCounter++) FindSourceImageFormat(itCounter, extInt);
00398                 if (extInt != "") m_IntensityImageFileNames = intensityImageFileNames[extInt];
00399 
00400                 // amplitude
00401                 std::string extAmp = "";
00402                 for (itCounter = amplitudeImageCounter.begin(); itCounter != amplitudeImageCounter.end(); itCounter++) FindSourceImageFormat(itCounter, extAmp);
00403                 if (extAmp != "") m_AmplitudeImageFileNames = amplitudeImageFileNames[extAmp];
00404 
00405                 // coordinates
00406                 std::string extCoord = "";
00407                 for (itCounter = coordinateImageCounter.begin(); itCounter != coordinateImageCounter.end(); itCounter++) FindSourceImageFormat(itCounter, extCoord);
00408                 if (extCoord != "") m_CoordinateImageFileNames = coordinateImageFileNames[extCoord];
00409 
00410                 // range
00411                 std::string extRange = "";
00412                 for (itCounter = rangeImageCounter.begin(); itCounter != rangeImageCounter.end(); itCounter++) FindSourceImageFormat(itCounter, extRange);
00413                 if (extRange != "") m_RangeImageFileNames = rangeImageFileNames[extRange];
00414 
00415                 std::sort(m_IntensityImageFileNames.begin(),m_IntensityImageFileNames.end());
00416                 std::sort(m_AmplitudeImageFileNames.begin(),m_AmplitudeImageFileNames.end());
00417                 std::sort(m_CoordinateImageFileNames.begin(),m_CoordinateImageFileNames.end());
00418                 std::sort(m_RangeImageFileNames.begin(),m_RangeImageFileNames.end());
00419                 std::cout << "INFO - VirtualRangeCam::Open:" << std::endl;
00420                 std::cout << "\t ... Extracted '" << intensityImageCounter[extInt] << "' intensity images (3*8 or 16 bit/value)\n";
00421                 std::cout << "\t ... Extracted '" << amplitudeImageCounter[extAmp] << "' amplitude images (16 bit/value)\n";
00422                 std::cout << "\t ... Extracted '" << coordinateImageCounter[extCoord] << "' coordinate images (3*16 bit/value)\n";
00423                 std::cout << "\t ... Extracted '" << rangeImageCounter[extRange] << "' range images (16 bit/value)\n";
00424 
00425                 if (intensityImageCounter[extInt] == 0 && amplitudeImageCounter[extAmp] == 0)
00426                 {
00427                         std::cerr << "ERROR - VirtualRangeCam::Open:" << std::endl;
00428                         std::cerr << "\t ... Could not detect any intensity or amplitude images" << std::endl;
00429                         std::cerr << "\t ... from the specified directory." << std::endl;
00430                         return ipa_CameraSensors::RET_FAILED;
00431                 }
00432 
00433                 if (coordinateImageCounter[extCoord] != 0 &&
00434                         ((intensityImageCounter[extInt] != coordinateImageCounter[extCoord] &&
00435                         amplitudeImageCounter[extAmp] != coordinateImageCounter[extCoord]) ||
00436                         (coordinateImageCounter[extCoord] != rangeImageCounter[extRange])))
00437                 {
00438                         std::cerr << "ERROR - VirtualRangeCam::Open:" << std::endl;
00439                         std::cerr << "\t ... Number of intensity, range and coordinate images must agree." << std::endl;
00440                         return ipa_CameraSensors::RET_FAILED;
00441                 }
00442 
00443                 if((m_CalibrationMethod == NATIVE || m_CalibrationMethod == MATLAB_NO_Z) && coordinateImageCounter[extCoord] == 0 )
00444                 {
00445                         std::cerr << "ERROR - VirtualRangeCam::Open:" << std::endl;
00446                         std::cerr << "\t ... Coordinate images must be available for calibration mode NATIVE or MATLAB_NO_Z." << std::endl;
00447                         return ipa_CameraSensors::RET_FAILED;
00448                 }
00449         }
00450         else
00451         {
00452                 std::cerr << "ERROR - VirtualRangeCam::Open():" << std::endl;
00453                 std::cerr << "\t ... Path '" << absoluteDirectoryName.file_string() << "' is not a directory." << std::endl;
00454                 return ipa_CameraSensors::RET_FAILED;
00455         }
00456 
00457 
00458         std::cout << "**************************************************" << std::endl;
00459         std::cout << "VirtualRangeCam::Open(): Virtual range camera OPEN" << std::endl;
00460         std::cout << "**************************************************" << std::endl<< std::endl;
00461         m_open = true;
00462 
00463         return RET_OK;
00464 }
00465 
00466 
00467 unsigned long VirtualRangeCam::Close()
00468 {
00469         if (!isOpen())
00470         {
00471                 return (RET_OK);
00472         }
00473 
00474         m_open = false;
00475         return RET_OK;
00476 
00477 }
00478 
00479 
00480 unsigned long VirtualRangeCam::SetProperty(t_cameraProperty* cameraProperty)
00481 {
00482         switch (cameraProperty->propertyID)
00483         {
00484                 case PROP_CAMERA_RESOLUTION:
00485 
00486                 default:
00487                         std::cerr << "ERROR - VirtualRangeCam::SetProperty:" << std::endl;
00488                         std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified.\n";
00489                         return RET_FAILED;
00490                         break;
00491         }
00492 
00493         return RET_OK;
00494 }
00495 
00496 
00497 unsigned long VirtualRangeCam::SetPropertyDefaults()
00498 {
00499         return RET_FUNCTION_NOT_IMPLEMENTED;
00500 }
00501 
00502 
00503 unsigned long VirtualRangeCam::GetProperty(t_cameraProperty* cameraProperty)
00504 {
00505         switch (cameraProperty->propertyID)
00506         {
00507                 case PROP_CAMERA_RESOLUTION:
00508                         cameraProperty->cameraResolution.xResolution = m_ImageWidth;
00509                         cameraProperty->cameraResolution.yResolution = m_ImageHeight;
00510                         cameraProperty->propertyType = TYPE_CAMERA_RESOLUTION;
00511                         break;
00512 
00513                 case PROP_DMA_BUFFER_SIZE:
00514                         cameraProperty->u_integerData = m_BufferSize;
00515                         return RET_OK;
00516                         break;
00517 
00518                 default:
00519                         std::cerr << "VirtualRangeCam::GetProperty:" << std::endl;
00520                         std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified.";
00521                         return RET_FAILED;
00522                         break;
00523 
00524         }
00525 
00526         return RET_OK;
00527 }
00528 
00529 
00530 // Wrapper for IplImage retrival from AcquireImage
00531 // Images have to be initialized prior to calling this function
00532 unsigned long VirtualRangeCam::AcquireImages(cv::Mat* rangeImage, cv::Mat* grayImage, cv::Mat* cartesianImage,
00533                                                                                          bool getLatestFrame, bool undistort, ipa_CameraSensors::t_ToFGrayImageType grayImageType)
00534 {
00535         //std::cout << m_ImageCounter << std::endl;
00536 
00537         char* rangeImageData = 0;
00538         char* grayImageData = 0;
00539         char* cartesianImageData = 0;
00540         int widthStepRange = -1;
00541         int widthStepGray = -1;
00542         int widthStepCartesian = -1;
00543 
00544         if(rangeImage)
00545         {
00546                 rangeImage->create(m_ImageHeight, m_ImageWidth, CV_32FC1);
00547                 rangeImageData = (char*) rangeImage->data;
00548                 widthStepRange = rangeImage->step;
00549         }
00550 
00551         if(grayImage)
00552         {
00553                 if (grayImageType == ipa_CameraSensors::INTENSITY_8U3) grayImage->create(m_ImageHeight, m_ImageWidth, CV_8UC3);
00554                 else grayImage->create(m_ImageHeight, m_ImageWidth, CV_32FC1);
00555                 grayImageData = (char*) grayImage->data;
00556                 widthStepGray = grayImage->step;
00557         }
00558 
00559         if(cartesianImage)
00560         {
00561                 cartesianImage->create(m_ImageHeight, m_ImageWidth, CV_32FC3);
00562                 cartesianImageData = (char*) cartesianImage->data;
00563                 widthStepCartesian = cartesianImage->step;
00564         }
00565 
00566         if (widthStepRange+widthStepGray+widthStepCartesian == -3)
00567         {
00568                 return RET_OK;
00569         }
00570 
00571         return AcquireImages(widthStepRange, widthStepGray, widthStepCartesian, rangeImageData, grayImageData, cartesianImageData, getLatestFrame, undistort, grayImageType);
00572 
00573 }
00574 
00575 unsigned long VirtualRangeCam::AcquireImages(int widthStepRange, int widthStepGray, int widthStepCartesian, char* rangeImageData, char* grayImageData, char* cartesianImageData,
00576                                                                                          bool getLatestFrame, bool undistort, ipa_CameraSensors::t_ToFGrayImageType grayImageType)
00577 {
00578         if (!m_open)
00579         {
00580                 std::cerr << "ERROR - VirtualRangeCam::AcquireImages:" << std::endl;
00581                 std::cerr << "\t ... Camera not open." << std::endl;
00582                 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00583         }
00584 
00586 // Range image (distorted or undistorted)
00588         if (rangeImageData)
00589         {
00590                 float* f_ptr = 0;
00591                 float* f_ptr_dst = 0;
00592                 bool releaseNecessary = false;
00593 
00594                 cv::Mat rangeMat;
00595                 IplImage rangeIpl;
00596                 IplImage* rangeImage = &rangeIpl;
00597                 if (m_RangeImageFileNames[m_ImageCounter].find(".bin") != std::string::npos)
00598                 {
00599                         ipa_Utils::LoadMat(rangeMat, m_RangeImageFileNames[m_ImageCounter]);
00600                         rangeIpl = (IplImage)rangeMat;
00601                 }
00602                 else if (m_RangeImageFileNames[m_ImageCounter].find(".xml") != std::string::npos)
00603                 {
00604                         rangeImage = (IplImage*) cvLoad(m_RangeImageFileNames[m_ImageCounter].c_str(), 0);
00605                         releaseNecessary = true;
00606                 }
00607                 else
00608                 {
00609                         std::cerr << "ERROR - VirtualRangeCam::AcquireImages:\n";
00610                         std::cerr << "\t ... Wrong file format for file " << m_RangeImageFileNames[m_ImageCounter] << ".\n";
00611                         CV_Assert(false);
00612                 }
00613                 
00614                 if (!undistort)
00615                 {
00616                         // put data in corresponding IPLImage structures
00617                         for(unsigned int row=0; row<(unsigned int)m_ImageHeight; row++)
00618                         {
00619                                 f_ptr = (float*) (rangeImage->imageData + row*rangeImage->widthStep);
00620                                 f_ptr_dst = (float*) (rangeImageData + row*widthStepRange);
00621 
00622                                 for (unsigned int col=0; col<(unsigned int)m_ImageWidth; col++)
00623                                 {
00624                                         f_ptr_dst[col] = f_ptr[col];
00625                                 }
00626                         }
00627                 }
00628                 else
00629                 {
00630                         cv::Mat undistortedData (m_ImageHeight, m_ImageWidth, CV_32FC1, (float*) rangeImageData);
00631                         cv::Mat cpp_rangeImage = rangeImage;
00632 
00633                         assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00634                         cv::remap(cpp_rangeImage, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00635                 }
00636 
00637                 if (releaseNecessary) cvReleaseImage(&rangeImage);
00638         } // End if (rangeImage)
00640 // Gray image based on amplitude or intensity (distorted or undistorted)
00642         if(grayImageData)
00643         {
00644                 float* f_ptr = 0;
00645                 float* f_ptr_dst = 0;
00646                 unsigned char* uc_ptr = 0;
00647                 unsigned char* uc_ptr_dst = 0;
00648                 cv::Mat grayMat;
00649                 IplImage grayIpl;
00650                 IplImage* grayImage = &grayIpl;
00651                 bool releaseNecessary = false;
00652 
00653                 // load image
00654                 if ((grayImageType == ipa_CameraSensors::INTENSITY_32F1) || (grayImageType == ipa_CameraSensors::INTENSITY_8U3))
00655                 {
00656                         // intensity image
00657                         if (m_IntensityImageFileNames[m_ImageCounter].find(".bin") != std::string::npos)
00658                         {
00659                                 ipa_Utils::LoadMat(grayMat, m_IntensityImageFileNames[m_ImageCounter]);
00660                                 grayIpl = (IplImage)grayMat;
00661                         }
00662                         else if (m_IntensityImageFileNames[m_ImageCounter].find(".xml") != std::string::npos)
00663                         {
00664                                 grayImage = (IplImage*) cvLoad(m_IntensityImageFileNames[m_ImageCounter].c_str());
00665                                 releaseNecessary = true;
00666                         }
00667                         else if ((m_IntensityImageFileNames[m_ImageCounter].find(".png") != std::string::npos) || (m_IntensityImageFileNames[m_ImageCounter].find(".bmp") != std::string::npos) ||
00668                                         (m_IntensityImageFileNames[m_ImageCounter].find(".jpg") != std::string::npos))
00669                         {
00670                                 grayMat = cv::imread(m_IntensityImageFileNames[m_ImageCounter], -1);
00671                                 grayIpl = (IplImage)grayMat;
00672                         }
00673                         else
00674                         {
00675                                 std::cerr << "ERROR - VirtualRangeCam::AcquireImages:\n";
00676                                 std::cerr << "\t ... Wrong file format for file " << m_IntensityImageFileNames[m_ImageCounter] << ".\n";
00677                                 CV_Assert(false);
00678                         }
00679                 }
00680                 else
00681                 {
00682                         // amplitude image
00683                         if (m_AmplitudeImageFileNames[m_ImageCounter].find(".bin") != std::string::npos)
00684                         {
00685                                 ipa_Utils::LoadMat(grayMat, m_AmplitudeImageFileNames[m_ImageCounter]);
00686                                 grayIpl = (IplImage)grayMat;
00687                         }
00688                         else if (m_AmplitudeImageFileNames[m_ImageCounter].find(".xml") != std::string::npos)
00689                         {
00690                                 grayImage = (IplImage*) cvLoad(m_AmplitudeImageFileNames[m_ImageCounter].c_str());
00691                                 releaseNecessary = true;
00692                         }
00693                         else
00694                         {
00695                                 std::cerr << "ERROR - VirtualRangeCam::AcquireImages:\n";
00696                                 std::cerr << "\t ... Wrong file format for file " << m_AmplitudeImageFileNames[m_ImageCounter] << ".\n";
00697                                 CV_Assert(false);
00698                         }
00699                 }
00700                 
00701                 // process image
00702                 if (!undistort)
00703                 {
00704                         if (grayImageType == ipa_CameraSensors::INTENSITY_8U3)
00705                         {
00706                                 for(unsigned int row=0; row<(unsigned int)m_ImageHeight; row++)
00707                                 {
00708                                         f_ptr = (float*) (grayImage->imageData + row*grayImage->widthStep);
00709                                         f_ptr_dst = (float*) (grayImageData + row*widthStepGray);
00710 
00711                                         for (unsigned int col=0; col<(unsigned int)m_ImageWidth; col++)
00712                                         {
00713                                                 f_ptr_dst[col] = f_ptr[col];
00714                                         }
00715                                 }
00716                         }
00717                         else
00718                         {
00719                                 for(unsigned int row=0; row<(unsigned int)m_ImageHeight; row++)
00720                                 {
00721                                         uc_ptr = (unsigned char*) (grayImage->imageData + row*grayImage->widthStep);
00722                                         uc_ptr_dst = (unsigned char*) (grayImageData + row*widthStepGray);
00723 
00724                                         for (unsigned int col=0; col<(unsigned int)m_ImageWidth; col++)
00725                                         {
00726                                                 uc_ptr_dst[col] = uc_ptr[col];
00727                                         }
00728                                 }
00729                         }
00730                 }
00731                 else
00732                 {
00733                         cv::Mat undistortedData;
00734                         if (grayImageType == ipa_CameraSensors::INTENSITY_8U3)
00735                         {
00736                                 undistortedData = cv::Mat(m_ImageHeight, m_ImageWidth, CV_8UC3, (unsigned char*) grayImageData);
00737                         }
00738                         else
00739                         {
00740                                 undistortedData = cv::Mat(m_ImageHeight, m_ImageWidth, CV_32FC1, (float*) grayImageData);
00741                         }
00742                         cv::Mat cpp_grayImage = grayImage;
00743 
00744                         assert (!m_undistortMapX.empty() && !m_undistortMapY.empty());
00745                         cv::remap(cpp_grayImage, undistortedData, m_undistortMapX, m_undistortMapY, cv::INTER_LINEAR);
00746                 }
00747 
00748                 if (releaseNecessary) cvReleaseImage(&grayImage);
00749         }
00751 // Cartesian image (always undistorted)
00753         if(cartesianImageData)
00754         {
00755                 float x = -1;
00756                 float y = -1;
00757                 float zCalibrated = -1;
00758                 float* f_ptr = 0;
00759                 float* f_ptr_dst = 0;
00760                 bool releaseNecessary = false;
00761 
00762                 if(m_CalibrationMethod==MATLAB)
00763                 {
00764                         CV_Assert(false);
00765                 }
00766                 else if(m_CalibrationMethod==MATLAB_NO_Z)
00767                 {
00768                         // XYZ image is assumed to be undistorted
00769                         // Unfortunately we have no access to the swissranger calibration
00770 
00771                         cv::Mat coordinateMat;
00772                         IplImage coordinateIpl;
00773                         IplImage* coordinateImage = &coordinateIpl;
00774                         if (m_CoordinateImageFileNames[m_ImageCounter].find(".bin") != std::string::npos)
00775                         {
00776                                 ipa_Utils::LoadMat(coordinateMat, m_CoordinateImageFileNames[m_ImageCounter]);
00777                                 coordinateIpl = (IplImage)coordinateMat;
00778                         }
00779                         else if (m_CoordinateImageFileNames[m_ImageCounter].find(".xml") != std::string::npos)
00780                         {
00781                                 coordinateImage = (IplImage*) cvLoad(m_CoordinateImageFileNames[m_ImageCounter].c_str(), 0);
00782                                 releaseNecessary = true;
00783                         }
00784                         else
00785                         {
00786                                 std::cerr << "ERROR - VirtualRangeCam::AcquireImages:\n";
00787                                 std::cerr << "\t ... Wrong file format for file " << m_CoordinateImageFileNames[m_ImageCounter] << ".\n";
00788                                 CV_Assert(false);
00789                         }
00790 
00791                         for(unsigned int row=0; row<(unsigned int)m_ImageHeight; row++)
00792                         {
00793                                 f_ptr = (float*) (coordinateImage->imageData + row*coordinateImage->widthStep);
00794                                 f_ptr_dst = (float*) (cartesianImageData + row*widthStepCartesian);
00795 
00796                                 for (unsigned int col=0; col<(unsigned int)m_ImageWidth; col++)
00797                                 {
00798                                         int colTimes3 = 3*col;
00799 
00800                                         zCalibrated = f_ptr[colTimes3+2];
00801                                         GetCalibratedXYMatlab(col, row, zCalibrated, x, y);
00802 
00803                                         f_ptr_dst[colTimes3] = x;
00804                                         f_ptr_dst[colTimes3 + 1] = y;
00805                                         f_ptr_dst[colTimes3 + 2] = zCalibrated;
00806 
00807                                         if (f_ptr_dst[colTimes3 + 2] < 0)
00808                                         {
00809                                                 std::cout << "<0: " << row << " " << col << "\n";
00810                                         }
00811                                 }
00812                         }
00813                         if (releaseNecessary) cvReleaseImage(&coordinateImage);
00814                 }
00815                 else if(m_CalibrationMethod==NATIVE)
00816                 {
00817                         cv::Mat coordinateMat;
00818                         IplImage coordinateIpl;
00819                         IplImage* coordinateImage = &coordinateIpl;
00820                         if (m_CoordinateImageFileNames[m_ImageCounter].find(".bin") != std::string::npos)
00821                         {
00822                                 ipa_Utils::LoadMat(coordinateMat, m_CoordinateImageFileNames[m_ImageCounter]);
00823                                 coordinateIpl = (IplImage)coordinateMat;
00824                         }
00825                         else if (m_CoordinateImageFileNames[m_ImageCounter].find(".xml") != std::string::npos)
00826                         {
00827                                 coordinateImage = (IplImage*) cvLoad(m_CoordinateImageFileNames[m_ImageCounter].c_str(), 0);
00828                                 releaseNecessary = true;
00829                         }
00830                         else
00831                         {
00832                                 std::cerr << "ERROR - VirtualRangeCam::AcquireImages:\n";
00833                                 std::cerr << "\t ... Wrong file format for file " << m_CoordinateImageFileNames[m_ImageCounter] << ".\n";
00834                                 CV_Assert(false);
00835                         }
00836 
00837                         for(unsigned int row=0; row<(unsigned int)m_ImageHeight; row++)
00838                         {
00839                                 f_ptr = (float*) (coordinateImage->imageData + row*coordinateImage->widthStep);
00840                                 f_ptr_dst = (float*) (cartesianImageData + row*widthStepCartesian);
00841 
00842                                 for (unsigned int col=0; col<(unsigned int)m_ImageWidth; col++)
00843                                 {
00844                                         int colTimes3 = 3*col;
00845 
00846                                         f_ptr_dst[colTimes3] =f_ptr[colTimes3+0];;
00847                                         f_ptr_dst[colTimes3 + 1] = f_ptr[colTimes3+1];
00848                                         f_ptr_dst[colTimes3 + 2] = f_ptr[colTimes3+2];
00849                                 }
00850                         }
00851                         if (releaseNecessary) cvReleaseImage(&coordinateImage);
00852                 }
00853                 else
00854                 {
00855                         std::cerr << "ERROR - VirtualRangeCam::AcquireImages:" << std::endl;
00856                         std::cerr << "\t ... Calibration method unknown.\n";
00857                         return RET_FAILED;
00858                 }
00859         }
00860 
00861         std::cout << m_ImageCounter << "        \r" << std::endl;
00862         m_ImageCounter++;
00863         if ((m_IntensityImageFileNames.size() != 0 && m_ImageCounter >= m_IntensityImageFileNames.size()) ||
00864                 (m_AmplitudeImageFileNames.size() != 0 && m_ImageCounter >= m_AmplitudeImageFileNames.size()) ||
00865                 (m_RangeImageFileNames.size() != 0 && m_ImageCounter >= m_RangeImageFileNames.size()) ||
00866                 (m_CoordinateImageFileNames.size() != 0 && m_ImageCounter >= m_CoordinateImageFileNames.size()))
00867         {
00868                 // Reset image counter
00869                 m_ImageCounter = 0;
00870         }
00871 
00872         return RET_OK;
00873 }
00874 
00875 int VirtualRangeCam::GetNumberOfImages()
00876 {
00877         if (m_IntensityImageFileNames.size() == 0 && 
00878                 m_AmplitudeImageFileNames.size() == 0 &&
00879                 m_RangeImageFileNames.size() == 0 && 
00880                 m_CoordinateImageFileNames.size() == 0)
00881         {
00882                 return 0;
00883         }
00884 
00885         int min=std::numeric_limits<int>::max();
00886 
00887         if (m_IntensityImageFileNames.size() != 0) min = (int)std::min((float)min, (float)m_IntensityImageFileNames.size());
00888         if (m_AmplitudeImageFileNames.size() != 0) min = (int)std::min((float)min, (float)m_AmplitudeImageFileNames.size());
00889         if (m_RangeImageFileNames.size() != 0) min = (int)std::min((float)min, (float)m_RangeImageFileNames.size());
00890         if (m_CoordinateImageFileNames.size() != 0) min = (int)std::min((float)min, (float)m_CoordinateImageFileNames.size());
00891 
00892         return min;
00893 }
00894 
00895 unsigned long VirtualRangeCam::SetPathToImages(std::string path) 
00896 {
00897         m_CameraDataDirectory = path;
00898         return ipa_Utils::RET_OK;
00899 }
00900 
00901 unsigned long VirtualRangeCam::SaveParameters(const char* filename)
00902 {
00903         return RET_FUNCTION_NOT_IMPLEMENTED;
00904 }
00905 
00906 
00907 // Assert, that <code>m_CoeffsA.size()</code> and <code>m_CoeffsB.size()</code> is initialized
00908 // Before calling <code>GetCalibratedZMatlab</code>
00909 unsigned long VirtualRangeCam::GetCalibratedZMatlab(int u, int v, float zRaw, float& zCalibrated)
00910 {
00911         double c[7] = {m_CoeffsA0.at<double>(v,u), m_CoeffsA1.at<double>(v,u), m_CoeffsA2.at<double>(v,u), 
00912                 m_CoeffsA3.at<double>(v,u), m_CoeffsA4.at<double>(v,u), m_CoeffsA5.at<double>(v,u), m_CoeffsA6.at<double>(v,u)};
00913         double y = 0;
00914 
00915         ipa_Utils::EvaluatePolynomial((double) zRaw, 6, &c[0], &y);
00916 
00917         zCalibrated = (float) y;
00918         return RET_OK;
00919 }
00920 
00921 unsigned long VirtualRangeCam::GetCalibratedXYMatlab(int u, int v, float z, float& x, float& y)
00922 {
00923         // Conversion form m to mm
00924         z *= 1000;
00925 
00926         // Use intrinsic camera parameters
00927         double fx, fy, cx, cy;
00928 
00929         fx = m_intrinsicMatrix.at<double>(0, 0);
00930         fy = m_intrinsicMatrix.at<double>(1, 1);
00931 
00932         cx = m_intrinsicMatrix.at<double>(0, 2);
00933         cy = m_intrinsicMatrix.at<double>(1, 2);
00934 
00935         // Fundamental equation: u = (fx*x)/z + cx
00936         if (fx == 0)
00937         {
00938                 std::cerr << "VirtualRangeCam::GetCalibratedXYMatlab:" << std::endl;
00939                 std::cerr << "\t ... fx is 0.\n";
00940                 return RET_FAILED;
00941         }
00942         x = (float) (z*(u-cx)/fx) ;
00943 
00944         // Fundamental equation: v = (fy*y)/z + cy
00945         if (fy == 0)
00946         {
00947                 std::cerr << "VirtualRangeCam::GetCalibratedXYMatlab:" << std::endl;
00948                 std::cerr << "\t ... fy is 0.\n";
00949                 return RET_FAILED;
00950         }
00951         y = (float) (z*(v-cy)/fy);
00952 
00953         // Conversion from mm to m
00954         x /= 1000;
00955         y /= 1000;
00956 
00957         return RET_OK;
00958 }
00959 
00960 unsigned long VirtualRangeCam::GetCalibratedUV(double x, double y, double z, double& u, double& v)
00961 {
00962         if(m_CalibrationMethod==MATLAB || m_CalibrationMethod==MATLAB_NO_Z)
00963         {
00964                 double fx, fy, cx, cy;
00965                 fx = m_intrinsicMatrix.at<double>(0, 0);
00966                 fy = m_intrinsicMatrix.at<double>(1, 1);
00967 
00968                 cx = m_intrinsicMatrix.at<double>(0, 2);
00969                 cy = m_intrinsicMatrix.at<double>(1, 2);
00970 
00971                 // Conversion from m to mm
00972                 x *= 1000;
00973                 y *= 1000;
00974                 z *= 1000;
00975 
00976                 // Fundamental equation: u = (fx*x)/z + cx
00977                 if (z == 0)
00978                 {
00979                         std::cerr << "ERROR - VirtualRangeCam::GetCalibratedUV:" << std::endl;
00980                         std::cerr << "\t ... z is 0.\n";
00981                         return RET_FAILED;
00982                 }
00983 
00984                 u = (fx*x)/z + cx;
00985                 v = (fy*y)/z + cy;
00986         }
00987         else if(m_CalibrationMethod==NATIVE)
00988         {
00989                 // implement me
00990                 std::cerr << "ERROR - VirtualRangeCam::GetCalibratedUV:" << std::endl;
00991                 std::cerr << "\t ... Function not implemented.\n";
00992                 return RET_FAILED;
00993         }
00994 
00995         // Maybe skip this part... JBK skipped this - the calling function has to check!!
00996 
00997         if(u<0) u=0;
00998         if(u>=m_ImageWidth) u=m_ImageWidth-1;
00999         if(v<0) v=0;
01000         if(v>=m_ImageHeight) v=m_ImageHeight-1;
01001 
01002         return RET_OK;
01003 }
01004 
01005 unsigned long VirtualRangeCam::LoadParameters(const char* filename, int cameraIndex)
01006 {
01007         // Load SwissRanger parameters.
01008         boost::shared_ptr<TiXmlDocument> p_configXmlDocument (new TiXmlDocument( filename ));
01009 
01010         if (!p_configXmlDocument->LoadFile())
01011         {
01012                 std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01013                 std::cerr << "\t ... Error while loading xml configuration file" << std::endl;
01014                 std::cerr << "\t ...(Check filename and syntax):" << std::endl;
01015                 std::cerr << "\t ... " << filename << std::endl;
01016                 return (RET_FAILED | RET_FAILED_OPEN_FILE);
01017         }
01018         std::cout << "INFO - VirtualRangeCam::LoadParameters:" << std::endl;
01019         std::cout << "\t ... Parsing xml configuration file" << std::endl;
01020         std::cout << "\t ... '" << filename << "'" << std::endl;
01021 
01022         if ( p_configXmlDocument )
01023         {
01024 
01025 //************************************************************************************
01026 //      BEGIN LibCameraSensors
01027 //************************************************************************************
01028                 // Tag element "LibCameraSensors" of Xml Inifile
01029                 TiXmlElement *p_xmlElement_Root = NULL;
01030                 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
01031 
01032                 if ( p_xmlElement_Root )
01033                 {
01034 
01035 //************************************************************************************
01036 //      BEGIN LibCameraSensors->VirtualRangeCam
01037 //************************************************************************************
01038                         // Tag element "VirtualRangeCam" of Xml Inifile
01039                         TiXmlElement *p_xmlElement_Root_VirtualRangeCam = NULL;
01040                         std::stringstream ss;
01041                         ss << "VirtualRangeCam_" << cameraIndex;
01042                         p_xmlElement_Root_VirtualRangeCam = p_xmlElement_Root->FirstChildElement( ss.str() );
01043                         if ( p_xmlElement_Root_VirtualRangeCam )
01044                         {
01045 
01046 //************************************************************************************
01047 //      BEGIN LibCameraSensors->VirtualRangeCam->CameraDataDirectory
01048 //************************************************************************************
01049                                 // Subtag element "IntrinsicParameters" of Xml Inifile
01050                                 TiXmlElement *p_xmlElement_Child = NULL;
01051                                 p_xmlElement_Child = p_xmlElement_Root_VirtualRangeCam->FirstChildElement( "CameraDataDirectory" );
01052                                 if ( p_xmlElement_Child )
01053                                 {
01054                                         // read and save value of attribute
01055                                         std::string tempString;
01056                                         if ( p_xmlElement_Child->QueryValueAttribute( "relativePath", &tempString ) != TIXML_SUCCESS)
01057                                         {
01058                                                 std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01059                                                 std::cerr << "\t ... Can't find attribute 'relativePath' of tag 'CameraDataDirectory'." << std::endl;
01060                                                 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01061                                         }
01062 
01063                                         m_CameraDataDirectory = m_CameraDataDirectory + tempString + "/";
01064                                 }
01065                                 else
01066                                 {
01067                                         std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl;
01068                                         std::cerr << "\t ... Can't find tag 'CameraDataDirectory'." << std::endl;
01069                                         return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01070                                 }
01071 
01072 //************************************************************************************
01073 //      BEGIN LibCameraSensors->VirtualRangeCam->CalibrationMethod
01074 //************************************************************************************
01075                                 // Subtag element "OperationMode" of Xml Inifile
01076                                 p_xmlElement_Child = NULL;
01077                                 p_xmlElement_Child = p_xmlElement_Root_VirtualRangeCam->FirstChildElement( "CalibrationMethod" );
01078                                 std::string tempString;
01079                                 if ( p_xmlElement_Child )
01080                                 {
01081                                         // read and save value of attribute
01082                                         if ( p_xmlElement_Child->QueryValueAttribute( "name", &tempString ) != TIXML_SUCCESS)
01083                                         {
01084                                                 std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01085                                                 std::cerr << "\t ... Can't find attribute 'name' of tag 'CalibrationMethod'." << std::endl;
01086                                                 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
01087                                         }
01088                                         if (tempString == "MATLAB") m_CalibrationMethod = MATLAB;
01089                                         else if (tempString == "MATLAB_NO_Z") m_CalibrationMethod = MATLAB_NO_Z;
01090                                         else if (tempString == "NATIVE") m_CalibrationMethod = NATIVE;
01091                                         else
01092                                         {
01093                                                 std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01094                                                 std::cerr << "\t ... Calibration mode " << tempString << " unspecified." << std::endl;
01095                                                 return (RET_FAILED);
01096                                         }
01097                                 }
01098                                 else
01099                                 {
01100                                         std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01101                                         std::cerr << "\t ... Can't find tag 'CalibrationMethod'." << std::endl;
01102                                         return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01103                                 }
01104                         }
01105 //************************************************************************************
01106 //      END LibCameraSensors->VirtualRangeCam
01107 //************************************************************************************
01108                         else
01109                         {
01110                                 std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01111                                 std::cerr << "\t ... Can't find tag '" << ss.str() << "'" << std::endl;
01112                                 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01113                         }
01114                 }
01115 
01116 //************************************************************************************
01117 //      END LibCameraSensors
01118 //************************************************************************************
01119                 else
01120                 {
01121                         std::cerr << "ERROR - VirtualRangeCam::LoadParameters:" << std::endl;
01122                         std::cerr << "\t ... Can't find tag 'LibCameraSensors'." << std::endl;
01123                         return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
01124                 }
01125         }
01126 
01127         std::cout << "\t ... Parsing xml calibration file ... [OK] \n";
01128 
01129         return RET_OK;
01130 }


cob_camera_sensors
Author(s): Jan Fischer
autogenerated on Thu Aug 27 2015 12:45:58