Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "../include/cob_camera_sensors/StdAfx.h"
00019
00020 #ifdef __LINUX__
00021 #include "cob_camera_sensors/VirtualColorCam.h"
00022 #include "tinyxml.h"
00023 #else
00024 #include "cob_driver/cob_camera_sensors/common/include/cob_camera_sensors/VirtualColorCam.h"
00025 #include "cob_vision/windows/src/extern/TinyXml/tinyxml.h"
00026 #endif
00027
00028 #include <opencv/highgui.h>
00029 #include <iostream>
00030
00031 namespace fs = boost::filesystem;
00032 using namespace ipa_CameraSensors;
00033
00034 __DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr ipa_CameraSensors::CreateColorCamera_VirtualCam()
00035 {
00036 return AbstractColorCameraPtr(new VirtualColorCam());
00037 }
00038
00039 VirtualColorCam::VirtualColorCam()
00040 {
00041 m_initialized = false;
00042 m_open = false;
00043
00044 m_BufferSize = 1;
00045
00046 m_ImageWidth = 0;
00047 m_ImageHeight = 0;
00048
00049 m_ImageCounter = 0;
00050 }
00051
00052 VirtualColorCam::~VirtualColorCam()
00053 {
00054 if (isOpen())
00055 {
00056 Close();
00057 }
00058 }
00059
00060
00061 unsigned long VirtualColorCam::Init(std::string directory, int cameraIndex)
00062 {
00063 if (isInitialized())
00064 {
00065 return (RET_OK | RET_CAMERA_ALREADY_INITIALIZED);
00066 }
00067
00068 m_CameraType = ipa_CameraSensors::CAM_VIRTUALCOLOR;
00069
00070
00071 m_CameraDataDirectory = directory;
00072 if (LoadParameters((directory + "cameraSensorsIni.xml").c_str(), cameraIndex) & RET_FAILED)
00073 {
00074 return (RET_FAILED | RET_INIT_CAMERA_FAILED);
00075 }
00076
00077 m_CameraIndex = cameraIndex;
00078
00079 m_initialized = true;
00080 return RET_OK;
00081
00082 }
00083
00084
00085 unsigned long VirtualColorCam::Open()
00086 {
00087 if (!isInitialized())
00088 {
00089 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl;
00090 std::cerr << "\t ... Camera not initialized." << std::endl;
00091 return (RET_FAILED);
00092 }
00093 m_open = false;
00094
00095 if (isOpen())
00096 {
00097 return (RET_OK | RET_CAMERA_ALREADY_OPEN);
00098 }
00099
00100
00101 std::stringstream ss;
00102 std::string sCameraIndex;
00103 ss << m_CameraIndex;
00104 ss >> sCameraIndex;
00105
00106 m_ImageWidth = -1;
00107 m_ImageHeight = -1;
00108
00109
00110 fs::path absoluteDirectoryName( m_CameraDataDirectory );
00111 if ( !fs::exists( absoluteDirectoryName ) )
00112 {
00113 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl;
00114 std::cerr << "\t ... Path '" << absoluteDirectoryName.file_string() << "' not found" << std::endl;
00115 return (ipa_CameraSensors::RET_FAILED | ipa_CameraSensors::RET_FAILED_OPEN_FILE);
00116 }
00117
00118 int colorImageCounter = 0;
00119
00120 if ( fs::is_directory( absoluteDirectoryName ) )
00121 {
00122 std::cout << "INFO - VirtualColorCam::Open:" << std::endl;
00123 std::cout << "\t ... Parsing directory '" << absoluteDirectoryName.directory_string() << "'" << std::endl;;
00124 fs::directory_iterator end_iter;
00125 for ( fs::directory_iterator dir_itr( absoluteDirectoryName ); dir_itr != end_iter; ++dir_itr )
00126 {
00127 try
00128 {
00129 if (fs::is_regular_file(dir_itr->status()))
00130 {
00131 std::string filename = dir_itr->path().string();
00132 if ((dir_itr->path().extension() == ".jpg" || dir_itr->path().extension() == ".jpe" ||
00133 dir_itr->path().extension() == ".jpeg" || dir_itr->path().extension() == ".bmp" ||
00134 dir_itr->path().extension() == ".bmp" || dir_itr->path().extension() == ".dib" ||
00135 dir_itr->path().extension() == ".png" || dir_itr->path().extension() == ".pgm" ||
00136 dir_itr->path().extension() == ".ppm" || dir_itr->path().extension() == ".sr" ||
00137 dir_itr->path().extension() == ".ras" || dir_itr->path().extension() == ".tiff" ||
00138 dir_itr->path().extension() == ".exr" || dir_itr->path().extension() == ".jp2") &&
00139 filename.find( "ColorCamRGB_8U3_" + sCameraIndex, 0 ) != std::string::npos)
00140 {
00141 ++colorImageCounter;
00142
00143 m_ColorImageFileNames.push_back(dir_itr->path().string());
00144
00145 if (m_ImageWidth == -1 || m_ImageHeight == -1)
00146 {
00147 cv::Mat image = cv::imread(m_ColorImageFileNames.back());
00148 m_ImageWidth = image.cols;
00149 m_ImageHeight = image.rows;
00150 }
00151 }
00152 }
00153 }
00154 catch ( const std::exception &ex )
00155 {
00156 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl;
00157 std::cerr << "\t ... Exception catch of '" << ex.what() << "'" << std::endl;
00158 }
00159 }
00160 std::sort(m_ColorImageFileNames.begin(),m_ColorImageFileNames.end());
00161 std::cout << "INFO - VirtualColorCam::Open:" << std::endl;
00162 std::cerr << "\t ... Extracted '" << colorImageCounter << "' color images (8*3 bit/color)\n";
00163 }
00164 else
00165 {
00166 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl;
00167 std::cerr << "\t .... Path '" << absoluteDirectoryName.file_string() << "' is not a directory." << std::endl;
00168 return ipa_CameraSensors::RET_FAILED;
00169 }
00170
00171 if (colorImageCounter == 0)
00172 {
00173 std::cerr << "ERROR - VirtualColorCam::Open:" << std::endl;
00174 std::cerr << "\t ... Could not detect any color images" << std::endl;
00175 std::cerr << "\t ... from the specified directory. Check directory" << std::endl;
00176 std::cerr << "\t ... and filenames (i.e. ColorCamRGB_8U3_*_*.jpg)." << std::endl;
00177 return ipa_CameraSensors::RET_FAILED;
00178 }
00179
00180 std::cout << "*******************************************************" << std::endl;
00181 std::cout << "VirtualColorCam::Open: Virtual color camera device OPEN" << std::endl;
00182 std::cout << "*******************************************************" << std::endl << std::endl;
00183
00184 m_open = true;
00185 return RET_OK;
00186
00187 }
00188
00189 int VirtualColorCam::GetNumberOfImages()
00190 {
00191 return (int)std::min(0.0f, (float)m_ColorImageFileNames.size());
00192 }
00193
00194 unsigned long VirtualColorCam::SaveParameters(const char* filename)
00195 {
00196 return RET_FAILED;
00197 }
00198
00199 unsigned long VirtualColorCam::SetPathToImages(std::string path)
00200 {
00201 m_CameraDataDirectory = path;
00202 return RET_OK;
00203 }
00204
00205
00206 unsigned long VirtualColorCam::Close()
00207 {
00208 if (!isOpen())
00209 {
00210 return RET_OK;
00211 }
00212
00213 m_open = false;
00214 return RET_OK;
00215 }
00216
00217
00218 unsigned long VirtualColorCam::SetProperty(t_cameraProperty* cameraProperty)
00219 {
00220 #ifdef __LINUX__
00221 std::cerr << "VirtualColorCam::SetProperty: Function not implemented.";
00222 return RET_FAILED;
00223 #endif
00224 #ifndef __LINUX__
00225
00226 switch (cameraProperty->propertyID)
00227 {
00228 case PROP_CAMERA_RESOLUTION:
00229 m_ImageWidth = cameraProperty->cameraResolution.xResolution;
00230 m_ImageHeight = cameraProperty->cameraResolution.yResolution;
00231 break;
00232 default:
00233 std::cerr << "ERROR - VirtualColorCam::SetProperty:" << std::endl;
00234 std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified.";
00235 return RET_FAILED;
00236 break;
00237 }
00238
00239 return RET_OK;
00240 #endif
00241 }
00242
00243 unsigned long VirtualColorCam::SetPropertyDefaults() {return RET_FUNCTION_NOT_IMPLEMENTED;}
00244
00245 unsigned long VirtualColorCam::GetProperty(t_cameraProperty* cameraProperty)
00246 {
00247 switch (cameraProperty->propertyID)
00248 {
00249 case PROP_CAMERA_RESOLUTION:
00250 cameraProperty->cameraResolution.xResolution = m_ImageWidth;
00251 cameraProperty->cameraResolution.yResolution = m_ImageHeight;
00252 cameraProperty->propertyType = TYPE_CAMERA_RESOLUTION;
00253 return RET_OK;
00254 break;
00255
00256 case PROP_DMA_BUFFER_SIZE:
00257 cameraProperty->u_integerData = m_BufferSize;
00258 return RET_OK;
00259 break;
00260
00261 default:
00262 std::cerr << "ERROR - VirtualColorCam::SetProperty:" << std::endl;
00263 std::cerr << "\t ... Property " << cameraProperty->propertyID << " unspecified.";
00264 return RET_FAILED;
00265 break;
00266
00267 }
00268
00269 return RET_OK;
00270 }
00271
00272
00273 unsigned long VirtualColorCam::GetColorImage(char* colorImageData, bool getLatestFrame)
00274 {
00275 if (!isOpen())
00276 {
00277 std::cerr << "ERROR - VirtualColorCam::GetColorImage:" << std::endl;
00278 std::cerr << "\t ... Color camera not open." << std::endl;
00279 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00280 }
00281
00282 IplImage* colorImage = (IplImage*) cvLoadImage(m_ColorImageFileNames[m_ImageCounter].c_str(), CV_LOAD_IMAGE_COLOR);
00283
00284 for(int row=0; row<m_ImageHeight; row++)
00285 {
00286 for (int col=0; col<m_ImageWidth; col++)
00287 {
00288 char* f_color_ptr = &((char*) (colorImage->imageData + row*colorImage->widthStep))[col*3];
00289 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 0] = f_color_ptr[0];
00290 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 1] = f_color_ptr[1];
00291 ((char*) (colorImageData + row*colorImage->widthStep))[col*3 + 2] = f_color_ptr[2];
00292 }
00293 }
00294
00295 cvReleaseImage(&colorImage);
00296
00297 m_ImageCounter++;
00298 if (m_ImageCounter >= m_ColorImageFileNames.size())
00299 {
00300
00301 m_ImageCounter = 0;
00302 }
00303
00304 return RET_OK;
00305 }
00306
00307
00308 unsigned long VirtualColorCam::GetColorImage(cv::Mat* colorImage, bool getLatestFrame)
00309 {
00310 if (!isOpen())
00311 {
00312 std::cerr << "ERROR - VirtualColorCam::GetColorImage:" << std::endl;
00313 std::cerr << "\t ... Color camera not open." << std::endl;
00314 return (RET_FAILED | RET_CAMERA_NOT_OPEN);
00315 }
00316
00317 CV_Assert(colorImage != 0);
00318
00319 colorImage->create(m_ImageHeight, m_ImageWidth, CV_8UC3);
00320
00321 return GetColorImage((char*)(colorImage->ptr<unsigned char>(0)), getLatestFrame);
00322
00323 return RET_FAILED;
00324 }
00325
00326 unsigned long VirtualColorCam::PrintCameraInformation()
00327 {
00328 return RET_FUNCTION_NOT_IMPLEMENTED;
00329 }
00330
00331 unsigned long VirtualColorCam::TestCamera(const char* filename)
00332 {
00333 if (AbstractColorCamera::TestCamera(filename) & RET_FAILED)
00334 {
00335 return RET_FAILED;
00336 }
00337
00338 return RET_OK;
00339 }
00340
00341
00342 unsigned long VirtualColorCam::LoadParameters(const char* filename, int cameraIndex)
00343 {
00344 boost::shared_ptr<TiXmlDocument> p_configXmlDocument (new TiXmlDocument( filename ));
00345 if (!p_configXmlDocument->LoadFile())
00346 {
00347 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl;
00348 std::cerr << "\t ... Error while loading xml configuration file (Check filename and syntax of the file):\n" << filename << std::endl;
00349 return (RET_FAILED | RET_FAILED_OPEN_FILE);
00350 }
00351 std::cout << "INFO - VirtualColorCam::LoadParameters:" << std::endl;
00352 std::cout << "\t ... Parsing xml configuration file:" << std::endl;
00353 std::cout << "\t ... '" << filename << "'" << std::endl;
00354
00355 if ( p_configXmlDocument )
00356 {
00357
00358
00359
00360
00361
00362 TiXmlElement *p_xmlElement_Root = NULL;
00363 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
00364 if ( p_xmlElement_Root )
00365 {
00366
00367
00368
00369
00370
00371 TiXmlElement *p_xmlElement_Root_VirtualColorCam = NULL;
00372 std::stringstream ss;
00373 ss << "VirtualColorCam_" << cameraIndex;
00374 p_xmlElement_Root_VirtualColorCam = p_xmlElement_Root->FirstChildElement( ss.str() );
00375 if ( p_xmlElement_Root_VirtualColorCam )
00376 {
00377
00378
00379
00380
00381
00382 TiXmlElement *p_xmlElement_Child = NULL;
00383 p_xmlElement_Child = p_xmlElement_Root_VirtualColorCam->FirstChildElement( "CameraDataDirectory" );
00384 if ( p_xmlElement_Child )
00385 {
00386
00387 std::string tempString;
00388 if ( p_xmlElement_Child->QueryValueAttribute( "relativePath", &tempString ) != TIXML_SUCCESS)
00389 {
00390 std::cerr << "VirtualColorCam::LoadParameters: Can't find attribute 'relativePath' of tag 'CameraDataDirectory'." << std::endl;
00391 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00392 }
00393
00394 m_CameraDataDirectory = m_CameraDataDirectory + tempString + "/";
00395 }
00396 else
00397 {
00398 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl;
00399 std::cerr << "\t ... Can't find tag 'CameraDataDirectory'." << std::endl;
00400 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00401 }
00402 }
00403
00404
00405
00406 else
00407 {
00408 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl;
00409 std::cerr << "\t ... Can't find tag '" << ss.str() << "'" << std::endl;
00410 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00411 }
00412 }
00413
00414
00415
00416
00417 else
00418 {
00419 std::cerr << "ERROR - VirtualColorCam::LoadParameters:" << std::endl;
00420 std::cerr << "\t ... Can't find tag 'LibCameraSensors'." << std::endl;
00421 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00422 }
00423 }
00424
00425 std::cout << "\t [OK] Parsing xml configuration file " << std::endl;
00426
00427 return RET_OK;
00428 }