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 #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
00105 m_CameraDataDirectory = directory;
00106
00107
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
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
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
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
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
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
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
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
00224 }
00225 else
00226 {
00227 m_CoeffsA6 = c_mat;
00228 cvReleaseMat(&c_mat);
00229 }
00230 }
00231
00232 m_CameraIndex = cameraIndex;
00233
00234
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
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
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;
00311 std::map<std::string, int> intensityImageCounter;
00312 std::map<std::string, int> coordinateImageCounter;
00313 std::map<std::string, int> rangeImageCounter;
00314 std::map<std::string, std::vector<std::string> > amplitudeImageFileNames;
00315 std::map<std::string, std::vector<std::string> > intensityImageFileNames;
00316 std::map<std::string, std::vector<std::string> > coordinateImageFileNames;
00317 std::map<std::string, std::vector<std::string> > rangeImageFileNames;
00318
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
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
00343 intensityImageFileNames[ext].push_back(filename);
00344 UpdateImageDimensionsOnFirstImage(filename, ext);
00345 }
00346 }
00347
00348
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
00356 amplitudeImageFileNames[ext].push_back(filename);
00357 UpdateImageDimensionsOnFirstImage(filename, ext);
00358 }
00359 }
00360
00361
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
00371 }
00372 }
00373
00374
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
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
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
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
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
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
00531
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
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
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
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 }
00640
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
00654 if ((grayImageType == ipa_CameraSensors::INTENSITY_32F1) || (grayImageType == ipa_CameraSensors::INTENSITY_8U3))
00655 {
00656
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
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
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
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
00769
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
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
00908
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
00924 z *= 1000;
00925
00926
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
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
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
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
00972 x *= 1000;
00973 y *= 1000;
00974 z *= 1000;
00975
00976
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
00990 std::cerr << "ERROR - VirtualRangeCam::GetCalibratedUV:" << std::endl;
00991 std::cerr << "\t ... Function not implemented.\n";
00992 return RET_FAILED;
00993 }
00994
00995
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
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
01027
01028
01029 TiXmlElement *p_xmlElement_Root = NULL;
01030 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
01031
01032 if ( p_xmlElement_Root )
01033 {
01034
01035
01036
01037
01038
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
01048
01049
01050 TiXmlElement *p_xmlElement_Child = NULL;
01051 p_xmlElement_Child = p_xmlElement_Root_VirtualRangeCam->FirstChildElement( "CameraDataDirectory" );
01052 if ( p_xmlElement_Child )
01053 {
01054
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
01074
01075
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
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
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
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 }