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