00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <cob_vision_utils/StdAfx.h>
00019 #ifdef __LINUX__
00020 #include "cob_vision_utils/CameraSensorToolbox.h"
00021
00022 #include "cob_vision_utils/VisionUtils.h"
00023 #include "tinyxml.h"
00024 #include <opencv2/imgproc/imgproc.hpp>
00025 #else
00026 #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/CameraSensorToolbox.h"
00027
00028 #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/VisionUtils.h"
00029 #endif
00030
00031 using namespace ipa_CameraSensors;
00032
00033 __DLL_LIBCAMERASENSORS__ CameraSensorToolboxPtr ipa_CameraSensors::CreateCameraSensorToolbox()
00034 {
00035 return CameraSensorToolboxPtr(new CameraSensorToolbox());
00036 }
00037
00038 CameraSensorToolbox::CameraSensorToolbox()
00039 {
00040 m_Initialized = false;
00041 }
00042
00043 CameraSensorToolbox::~CameraSensorToolbox()
00044 {
00045 Release();
00046 }
00047
00048 unsigned long CameraSensorToolbox::Release()
00049 {
00050 std::map<std::string, CvMat*>::iterator matrixIterator;
00051 std::map<std::string, IplImage*>::iterator iplImageIterator;
00052
00053 m_intrinsicMatrices.clear();
00054 m_distortionCoeffs.clear();
00055 m_undistortMapsX.clear();
00056 m_undistortMapsY.clear();
00057 m_extrinsicMatrices.clear();
00058
00059 return RET_OK;
00060 }
00061
00062 CameraSensorToolbox::CameraSensorToolbox(const CameraSensorToolbox& cst)
00063 {
00064 Release();
00065
00066 std::map<std::string, cv::Mat>::const_iterator matrixIterator;
00067
00068
00069 for ( matrixIterator=cst.m_intrinsicMatrices.begin() ; matrixIterator != cst.m_intrinsicMatrices.end(); matrixIterator++ )
00070 {
00071 m_intrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00072 }
00073
00074
00075 for ( matrixIterator=cst.m_distortionCoeffs.begin() ; matrixIterator != cst.m_distortionCoeffs.end(); matrixIterator++ )
00076 {
00077 m_distortionCoeffs[matrixIterator->first] = matrixIterator->second.clone();
00078 }
00079
00080
00081 for ( matrixIterator=cst.m_undistortMapsX.begin() ; matrixIterator != cst.m_undistortMapsX.end(); matrixIterator++ )
00082 {
00083 m_undistortMapsX[matrixIterator->first] = matrixIterator->second.clone();
00084 }
00085
00086
00087 for ( matrixIterator=cst.m_undistortMapsY.begin() ; matrixIterator != cst.m_undistortMapsY.end(); matrixIterator++ )
00088 {
00089 m_undistortMapsY[matrixIterator->first] = matrixIterator->second.clone();
00090 }
00091
00092
00093 for ( matrixIterator=cst.m_extrinsicMatrices.begin() ; matrixIterator != cst.m_extrinsicMatrices.end(); matrixIterator++ )
00094 {
00095 m_extrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00096 }
00097
00098 m_Initialized = cst.m_Initialized;
00099 }
00100
00101 CameraSensorToolbox& CameraSensorToolbox::operator=(const CameraSensorToolbox& cst)
00102 {
00103
00104 if (this==&cst)
00105 {
00106 return *this;
00107 }
00108
00109 Release();
00110
00111 std::map<std::string, cv::Mat>::const_iterator matrixIterator;
00112
00113
00114 for ( matrixIterator=cst.m_intrinsicMatrices.begin() ; matrixIterator != cst.m_intrinsicMatrices.end(); matrixIterator++ )
00115 {
00116 m_intrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00117 }
00118
00119
00120 for ( matrixIterator=cst.m_distortionCoeffs.begin() ; matrixIterator != cst.m_distortionCoeffs.end(); matrixIterator++ )
00121 {
00122 m_distortionCoeffs[matrixIterator->first] = matrixIterator->second.clone();
00123 }
00124
00125
00126 for ( matrixIterator=cst.m_undistortMapsX.begin() ; matrixIterator != cst.m_undistortMapsX.end(); matrixIterator++ )
00127 {
00128 m_undistortMapsX[matrixIterator->first] = matrixIterator->second.clone();
00129 }
00130
00131
00132 for ( matrixIterator=cst.m_undistortMapsY.begin() ; matrixIterator != cst.m_undistortMapsY.end(); matrixIterator++ )
00133 {
00134 m_undistortMapsY[matrixIterator->first] = matrixIterator->second.clone();
00135 }
00136
00137
00138 for ( matrixIterator=cst.m_extrinsicMatrices.begin() ; matrixIterator != cst.m_extrinsicMatrices.end(); matrixIterator++ )
00139 {
00140 m_extrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00141 }
00142
00143 m_Initialized = cst.m_Initialized;
00144
00145 return *this;
00146 }
00147
00148 unsigned long CameraSensorToolbox::Init(std::string directory, ipa_CameraSensors::t_cameraType cameraType,
00149 int cameraIndex, const CvSize imageSize)
00150 {
00151 Release();
00152
00153 m_ImageSize = imageSize;
00154
00155 std::string iniFileNameAndPath = directory;
00156 iniFileNameAndPath += "cameraSensorsIni.xml";
00157 if (LoadParameters(iniFileNameAndPath.c_str(), cameraType, cameraIndex) & RET_FAILED)
00158 {
00159 return (RET_FAILED | RET_INIT_CAMERA_FAILED);
00160 }
00161
00162 m_Initialized = true;
00163 return RET_OK;
00164 }
00165
00166 unsigned long CameraSensorToolbox::Init(const std::map<std::string, cv::Mat>* intrinsicMatrices,
00167 const std::map<std::string, cv::Mat>* distortionParameters,
00168 const std::map<std::string, cv::Mat>* extrinsicMatrices,
00169 const std::map<std::string, cv::Mat>* undistortMapsX,
00170 const std::map<std::string, cv::Mat>* undistortMapsY,
00171 const CvSize imageSize)
00172 {
00173 Release();
00174
00175 m_ImageSize = imageSize;
00176
00177 std::map<std::string, cv::Mat>::const_iterator matrixIterator;
00178
00179
00180 for ( matrixIterator=m_intrinsicMatrices.begin() ; matrixIterator != m_intrinsicMatrices.end(); matrixIterator++ )
00181 {
00182 m_intrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00183 }
00184
00185
00186 for ( matrixIterator=m_distortionCoeffs.begin() ; matrixIterator != m_distortionCoeffs.end(); matrixIterator++ )
00187 {
00188 m_distortionCoeffs[matrixIterator->first] = matrixIterator->second.clone();
00189 }
00190
00191
00192 for ( matrixIterator=m_undistortMapsX.begin() ; matrixIterator != m_undistortMapsX.end(); matrixIterator++ )
00193 {
00194 m_undistortMapsX[matrixIterator->first] = matrixIterator->second.clone();
00195 }
00196
00197
00198 for ( matrixIterator=m_undistortMapsY.begin() ; matrixIterator != m_undistortMapsY.end(); matrixIterator++ )
00199 {
00200 m_undistortMapsY[matrixIterator->first] = matrixIterator->second.clone();
00201 }
00202
00203
00204 for ( matrixIterator=m_extrinsicMatrices.begin() ; matrixIterator != m_extrinsicMatrices.end(); matrixIterator++ )
00205 {
00206 m_extrinsicMatrices[matrixIterator->first] = matrixIterator->second.clone();
00207 }
00208
00209 m_Initialized = true;
00210 return RET_OK;
00211 }
00212
00213 unsigned long CameraSensorToolbox::ConvertCameraTypeToString(ipa_CameraSensors::t_cameraType cameraType, std::string &cameraTypeString)
00214 {
00215 switch (cameraType)
00216 {
00217 case ROBOT:
00218 cameraTypeString = "Robot";
00219 break;
00220 case CAM_IC:
00221 cameraTypeString = "ICCam";
00222 break;
00223 case CAM_AVTPIKE:
00224 cameraTypeString = "AVTPikeCam";
00225 break;
00226 case CAM_AXIS:
00227 cameraTypeString = "AxisCam";
00228 break;
00229 case CAM_PROSILICA:
00230 cameraTypeString = "Prosilica";
00231 break;
00232 case CAM_VIRTUALCOLOR:
00233 cameraTypeString = "VirtualColorCam";
00234 break;
00235 case CAM_SWISSRANGER:
00236 cameraTypeString = "Swissranger";
00237 break;
00238 case CAM_PMDCAM:
00239 cameraTypeString = "PMDCam";
00240 break;
00241 case CAM_KINECT:
00242 cameraTypeString = "Kinect";
00243 break;
00244 case CAM_OPENCVCAMERA:
00245 cameraTypeString = "OpenCVCamera";
00246 break;
00247 case CAM_VIRTUALRANGE:
00248 cameraTypeString = "VirtualRangeCam";
00249 break;
00250 case CAM_ENSENSO_N30:
00251 cameraTypeString = "EnsensoN30";
00252 break;
00253 case CAM_IDS_UEYE:
00254 cameraTypeString = "IDSuEyeCamera";
00255 break;
00256 case CAM_ENSENSO_IDS_RACK:
00257 cameraTypeString = "EnsensoIDSColorRack";
00258 break;
00259 case CAM_SOFTKINETIC:
00260 cameraTypeString = "SoftkineticCamera";
00261 break;
00262 default:
00263 std::cerr << "ERROR - CameraSensorToolbox::ConvertCameraTypeToString:" << std::endl;
00264 std::cerr << "\t ... Camera type " << cameraType << " unspecified." << std::endl;
00265 return RET_FAILED;
00266 }
00267
00268 return RET_OK;
00269 }
00270
00271 cv::Mat CameraSensorToolbox::GetExtrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00272 {
00273 std::stringstream ss;
00274 std::string extrinsicMapName = "";
00275
00276 ConvertCameraTypeToString(cameraType, extrinsicMapName);
00277 ss << extrinsicMapName << "_" << cameraIndex;
00278
00279 if (m_extrinsicMatrices.find(ss.str()) == m_extrinsicMatrices.end())
00280 {
00281 std::cout << "ERROR - CameraSensorToolbox::GetExtrinsicParameters:" << std::endl;
00282 std::cout << "\t ... Extrinsic matrix to '" << ss.str() << "' not specified\n";
00283 return cv::Mat();
00284 }
00285 else
00286 {
00287 return m_extrinsicMatrices[ss.str()];
00288 }
00289 }
00290
00291 unsigned long CameraSensorToolbox::SetExtrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex,
00292 const cv::Mat& _rotation, const cv::Mat& _translation)
00293 {
00294 std::stringstream ss;
00295 std::string extrinsicMapName = "";
00296
00297 ConvertCameraTypeToString(cameraType, extrinsicMapName);
00298 ss << extrinsicMapName << "_" << cameraIndex;
00299
00300 return SetExtrinsicParameters(ss.str(), _rotation, _translation);
00301 }
00302
00303 unsigned long CameraSensorToolbox::SetExtrinsicParameters(std::string key,
00304 const cv::Mat& _rotation, const cv::Mat& _translation)
00305 {
00306 CV_Assert( _rotation.rows == 3 && _rotation.cols == 3 && _rotation.depth() == CV_64FC(1));
00307 CV_Assert( _translation.rows == 3 && _translation.cols == 1 && _translation.depth() == CV_64FC(1));
00308
00309 std::map<std::string, cv::Mat>::iterator iterator;
00310 iterator = m_extrinsicMatrices.find(key);
00311 if (iterator != m_extrinsicMatrices.end())
00312 {
00313 m_extrinsicMatrices.erase(iterator);
00314 }
00315
00316 cv::Mat extrinsicMatrix(3, 4, CV_64FC(1), cv::Scalar(0));
00317
00318 extrinsicMatrix.at<double>(0, 0) = _rotation.at<double>(0, 0);
00319 extrinsicMatrix.at<double>(0, 1) = _rotation.at<double>(0, 1);
00320 extrinsicMatrix.at<double>(0, 2) = _rotation.at<double>(0, 2);
00321 extrinsicMatrix.at<double>(1, 0) = _rotation.at<double>(1, 0);
00322 extrinsicMatrix.at<double>(1, 1) = _rotation.at<double>(1, 1);
00323 extrinsicMatrix.at<double>(1, 2) = _rotation.at<double>(1, 2);
00324 extrinsicMatrix.at<double>(2, 0) = _rotation.at<double>(2, 0);
00325 extrinsicMatrix.at<double>(2, 1) = _rotation.at<double>(2, 1);
00326 extrinsicMatrix.at<double>(2, 2) = _rotation.at<double>(2, 2);
00327
00328 extrinsicMatrix.at<double>(0, 3) = _translation.at<double>(0, 0);
00329 extrinsicMatrix.at<double>(1, 3) = _translation.at<double>(1, 0);
00330 extrinsicMatrix.at<double>(2, 3) = _translation.at<double>(2, 0);
00331
00332 m_extrinsicMatrices[key] = extrinsicMatrix;
00333
00334 return RET_OK;
00335 }
00336
00337 cv::Mat CameraSensorToolbox::GetIntrinsicMatrix(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00338 {
00339 std::stringstream ss;
00340 std::string intrinsicMapName = "";
00341
00342 ConvertCameraTypeToString(cameraType, intrinsicMapName);
00343 ss << intrinsicMapName << "_" << cameraIndex;
00344
00345 if (m_intrinsicMatrices.find(ss.str()) == m_intrinsicMatrices.end())
00346 {
00347 std::cout << "ERROR - CameraSensorToolbox::GetIntrinsicMatrix:" << std::endl;
00348 std::cout << "\t ... Intrinsic matrix related to '" << ss.str() << "' not specified\n";
00349 return cv::Mat();
00350 }
00351 else
00352 {
00353 return m_intrinsicMatrices[ss.str()];
00354 }
00355 }
00356
00357 unsigned long CameraSensorToolbox::SetIntrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex,
00358 const cv::Mat& _intrinsicMatrix, const cv::Mat& _distortionCoeffs)
00359 {
00360 std::stringstream ss;
00361 std::string intrinsicMapName = "";
00362
00363 ConvertCameraTypeToString(cameraType, intrinsicMapName);
00364 ss << intrinsicMapName << "_" << cameraIndex;
00365
00366 return SetIntrinsicParameters(ss.str(), _intrinsicMatrix, _distortionCoeffs);
00367 }
00368
00369 unsigned long CameraSensorToolbox::SetIntrinsicParameters(std::string key,
00370 const cv::Mat& _intrinsicMatrix, const cv::Mat& _distortionCoeffs)
00371 {
00372 CV_Assert( _intrinsicMatrix.rows == 4 && _intrinsicMatrix.cols == 1 && _intrinsicMatrix.depth() == CV_64FC(1));
00373 CV_Assert( _distortionCoeffs.rows == 4 && _distortionCoeffs.cols == 1 && _distortionCoeffs.depth() == CV_64FC(1));
00374
00375 std::map<std::string, cv::Mat>::iterator matrixIterator;
00376
00377
00378
00379 matrixIterator = m_intrinsicMatrices.find(key);
00380 if (matrixIterator != m_intrinsicMatrices.end())
00381 {
00382 m_intrinsicMatrices.erase(matrixIterator);
00383 }
00384
00385 cv::Mat intrinsicMatrix (3, 3, CV_64FC(1), cv::Scalar(0) );
00386
00387 intrinsicMatrix.at<double>(0, 0) = _intrinsicMatrix.at<double>(0, 0);
00388 intrinsicMatrix.at<double>(1, 1) = _intrinsicMatrix.at<double>(1, 0);
00389 intrinsicMatrix.at<double>(0, 2) = _intrinsicMatrix.at<double>(2, 0);
00390 intrinsicMatrix.at<double>(1, 2) = _intrinsicMatrix.at<double>(3, 0);
00391 intrinsicMatrix.at<double>(2, 2) = 1.f;
00392
00393 m_intrinsicMatrices[key] = intrinsicMatrix;
00394
00395
00396 matrixIterator = m_distortionCoeffs.find(key);
00397 if (matrixIterator != m_distortionCoeffs.end())
00398 {
00399 m_distortionCoeffs.erase(matrixIterator);
00400 }
00401
00402 cv::Mat distortionCoeffs(1, 4, CV_64FC(1), cv::Scalar(0) );
00403
00404 distortionCoeffs.at<double>(0, 0) = _distortionCoeffs.at<double>(0, 0);
00405 distortionCoeffs.at<double>(0, 1) = _distortionCoeffs.at<double>(1, 0);
00406 distortionCoeffs.at<double>(0, 2) = _distortionCoeffs.at<double>(2, 0);
00407 distortionCoeffs.at<double>(0, 3) = _distortionCoeffs.at<double>(3, 0);
00408
00409 m_distortionCoeffs[key] = distortionCoeffs;
00410
00411
00412 matrixIterator = m_undistortMapsX.find(key);
00413 if (matrixIterator != m_undistortMapsX.end())
00414 {
00415 m_undistortMapsX.erase(matrixIterator);
00416 }
00417
00418 cv::Mat undistortMapX(m_ImageSize.height, m_ImageSize.width, CV_32FC(1));
00419
00420 matrixIterator = m_undistortMapsY.find(key);
00421 if (matrixIterator != m_undistortMapsY.end())
00422 {
00423 m_undistortMapsY.erase(matrixIterator);
00424 }
00425
00426 cv::Mat undistortMapY(m_ImageSize.height, m_ImageSize.width, CV_32FC(1));
00427
00428 ipa_Utils::InitUndistortMap(intrinsicMatrix, distortionCoeffs, undistortMapX, undistortMapY);
00429
00430 m_undistortMapsX[key] = undistortMapX;
00431 m_undistortMapsY[key] = undistortMapY;
00432
00433 return RET_OK;
00434 }
00435
00436 cv::Mat CameraSensorToolbox::GetDistortionParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00437 {
00438 std::stringstream ss;
00439 std::string distortionMapName = "";
00440
00441 ConvertCameraTypeToString(cameraType, distortionMapName);
00442 ss << distortionMapName << "_" << cameraIndex;
00443
00444 if (m_distortionCoeffs.find(ss.str()) == m_distortionCoeffs.end())
00445 {
00446 std::cout << "ERROR - CameraSensorToolbox::GetDistortionParameters:" << std::endl;
00447 std::cout << "\t ... Distortion parameters related to '" << ss.str() << "' not specified\n";
00448 return cv::Mat();
00449 }
00450 else
00451 {
00452 return m_distortionCoeffs[ss.str()];
00453 }
00454 }
00455
00456 cv::Mat CameraSensorToolbox::GetDistortionMapX(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00457 {
00458 std::stringstream ss;
00459 std::string distortionMapName = "";
00460
00461 ConvertCameraTypeToString(cameraType, distortionMapName);
00462 ss << distortionMapName << "_" << cameraIndex;
00463
00464 if (m_undistortMapsX.find(ss.str()) == m_undistortMapsX.end())
00465 {
00466 std::cout << "ERROR - CameraSensorToolbox::GetDistortionMapX:" << std::endl;
00467 std::cout << "\t ... Undistortion map X related to '" << ss.str() << "' not specified\n";
00468 return cv::Mat();
00469 }
00470 else
00471 {
00472 return m_undistortMapsX[ss.str()];
00473 }
00474 }
00475
00476 cv::Mat CameraSensorToolbox::GetDistortionMapY(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00477 {
00478 std::stringstream ss;
00479 std::string distortionMapName = "";
00480
00481 ConvertCameraTypeToString(cameraType, distortionMapName);
00482 ss << distortionMapName << "_" << cameraIndex;
00483
00484 if (m_undistortMapsY.find(ss.str()) == m_undistortMapsY.end())
00485 {
00486 std::cout << "ERROR - CameraSensorToolbox::GetDistortionMapY:" << std::endl;
00487 std::cout << "\t ... Undistortion map Y related to '" << ss.str() << "' not specified\n";
00488 return cv::Mat();
00489 }
00490 else
00491 {
00492 return m_undistortMapsY[ss.str()];
00493 }
00494 }
00495
00496 unsigned long CameraSensorToolbox::RemoveDistortion(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, const cv::Mat& src, cv::Mat& dst)
00497 {
00498 std::stringstream ss;
00499 std::string distortionMapName = "";
00500
00501 ConvertCameraTypeToString(cameraType, distortionMapName);
00502 ss << distortionMapName << "_" << cameraIndex;
00503
00504 if (m_undistortMapsX.find(ss.str()) == m_undistortMapsX.end() ||
00505 m_undistortMapsY.find(ss.str()) == m_undistortMapsY.end())
00506 {
00507 std::cout << "ERROR - CameraSensorToolbox::RemoveDistortion:" << std::endl;
00508 std::cout << "\t ... Undistortion map Y related to '" << ss.str() << "' not specified\n";
00509 return RET_FAILED;
00510 }
00511 else
00512 {
00513 CV_Assert(src.rows == m_undistortMapsX[ss.str()].rows && src.cols == m_undistortMapsX[ss.str()].cols);
00514
00515 cv::remap(src, dst, m_undistortMapsX[ss.str()], m_undistortMapsY[ss.str()], cv::INTER_LINEAR);
00516 return RET_OK;
00517 }
00518
00519 return (RET_FAILED | RET_MISSING_INTRINSIC_DISTORTION_PARAMS);
00520 }
00521
00522 unsigned long CameraSensorToolbox::ReprojectXYZ(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, double x, double y, double z, int& u, int& v)
00523 {
00524 std::stringstream ss;
00525 std::string distortionMapName = "";
00526
00527 ConvertCameraTypeToString(cameraType, distortionMapName);
00528 ss << distortionMapName << "_" << cameraIndex;
00529
00530 if (m_intrinsicMatrices.find(ss.str()) == m_intrinsicMatrices.end())
00531 {
00532 std::cout << "ERROR - CameraSensorToolbox::ReprojectXYZ:" << std::endl;
00533 std::cout << "\t ... Intrinsic matrix related to '" << ss.str() << "' not specified\n";
00534 return RET_FAILED;
00535 }
00536
00537 cv::Mat UV1 (3, 1, CV_64FC(1), cv::Scalar(0));
00538 cv::Mat XYZ (3, 1, CV_64FC(1), cv::Scalar(0));
00539
00540 x *= 1000;
00541 y *= 1000;
00542 z *= 1000;
00543
00544 x = x/z;
00545 y = y/z;
00546 z = 1;
00547
00548 XYZ.at<double>(0, 0) = x;
00549 XYZ.at<double>(1, 0) = y;
00550 XYZ.at<double>(2, 0) = z;
00551
00552
00553 if (z == 0)
00554 {
00555 std::cerr << "ERROR - CameraSensorToolbox::ReprojectXYZ" << std::endl;
00556 std::cerr << "\t ... z value is 0.\n";
00557 return RET_FAILED;
00558 }
00559
00560 UV1 = m_intrinsicMatrices[ss.str()] * XYZ;
00561
00562 u = cvRound(UV1.at<double>(0, 0));
00563 v = cvRound(UV1.at<double>(1, 0));
00564
00565 return RET_OK;
00566 }
00567
00568
00569 unsigned long CameraSensorToolbox::LoadParameters(const char* filename, ipa_CameraSensors::t_cameraType cameraType, int cameraIndex)
00570 {
00571 std::stringstream ss;
00572 std::string xmlTagName = "";
00573
00574 ConvertCameraTypeToString(cameraType, xmlTagName);
00575 ss << xmlTagName << "_" << cameraIndex;
00576
00577 boost::shared_ptr<TiXmlDocument> p_configXmlDocument (new TiXmlDocument( filename ));
00578 if (!p_configXmlDocument->LoadFile())
00579 {
00580 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00581 std::cerr << "\t ... Error while loading xml configuration file (Check filename and syntax of the file):\n" << filename << std::endl;
00582 return (RET_FAILED | RET_FAILED_OPEN_FILE);
00583 }
00584 std::cout << "INFO - CameraSensorsToolbox::LoadParameters:" << std::endl;
00585 std::cout << "\t ... Parsing xml configuration file:" << std::endl;
00586 std::cout << "\t ... '" << filename << "'" << std::endl;
00587
00588 if ( p_configXmlDocument )
00589 {
00590
00591
00592
00593
00594
00595 TiXmlElement *p_xmlElement_Root = NULL;
00596 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "LibCameraSensors" );
00597 if ( p_xmlElement_Root )
00598 {
00599
00600
00601
00602
00603 TiXmlElement *p_xmlElement_Root_Cam = NULL;
00604 p_xmlElement_Root_Cam = p_xmlElement_Root->FirstChildElement( ss.str() );
00605 if ( p_xmlElement_Root_Cam )
00606 {
00607
00608
00609
00610
00611 TiXmlElement *p_xmlElement_Child = NULL;
00612 p_xmlElement_Child = p_xmlElement_Root_Cam->FirstChildElement( "IntrinsicParameters" );
00613 if ( p_xmlElement_Child )
00614 {
00615 TiXmlElement *p_xmlElement_Intrinsics = 0;
00616 TiXmlElement *p_xmlElement_Intrinsics_Child = 0;
00617
00618 for( p_xmlElement_Intrinsics = p_xmlElement_Child->FirstChildElement();
00619 p_xmlElement_Intrinsics;
00620 p_xmlElement_Intrinsics = p_xmlElement_Intrinsics->NextSiblingElement())
00621 {
00622
00623
00624
00625
00626 cv::Mat intrinsicMatrix (4, 1, CV_64FC(1));
00627 p_xmlElement_Intrinsics_Child = NULL;
00628 p_xmlElement_Intrinsics_Child = p_xmlElement_Intrinsics->FirstChildElement( "IntrinsicMatrix" );
00629
00630 if ( p_xmlElement_Intrinsics_Child )
00631 {
00632 double fx, fy, cx, cy;
00633
00634 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "fx", &fx ) != TIXML_SUCCESS)
00635 {
00636 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00637 std::cerr << "\t ... Can't find attribute 'fx' of tag 'IntrinsicMatrix'." << std::endl;
00638 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00639 }
00640 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "fy", &fy ) != TIXML_SUCCESS)
00641 {
00642 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00643 std::cerr << "\t ... Can't find attribute 'fy' of tag 'IntrinsicMatrix'." << std::endl;
00644 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00645 }
00646 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "cx", &cx ) != TIXML_SUCCESS)
00647 {
00648 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00649 std::cerr << "\t ... Can't find attribute 'cx' of tag 'IntrinsicMatrix'." << std::endl;
00650 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00651 }
00652 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "cy", &cy ) != TIXML_SUCCESS)
00653 {
00654 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00655 std::cerr << "\t ... Can't find attribute 'cy' of tag 'IntrinsicMatrix'." << std::endl;
00656 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00657 }
00658
00659
00660 intrinsicMatrix.at<double>(0, 0) = fx;
00661 intrinsicMatrix.at<double>(1, 0) = fy;
00662 intrinsicMatrix.at<double>(2, 0) = cx;
00663 intrinsicMatrix.at<double>(3, 0) = cy;
00664 }
00665 else
00666 {
00667 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00668 std::cerr << "\t ... Can't find tag 'IntrinsicMatrix'." << std::endl;
00669 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00670 }
00671
00672
00673
00674
00675 cv::Mat distortionCoeffs (4, 1, CV_64FC(1));
00676 p_xmlElement_Intrinsics_Child = NULL;
00677 p_xmlElement_Intrinsics_Child = p_xmlElement_Intrinsics->FirstChildElement( "DistortionCoeffs" );
00678
00679 if ( p_xmlElement_Child )
00680 {
00681 double k1, k2, p1, p2;
00682
00683 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "k1", &k1 ) != TIXML_SUCCESS)
00684 {
00685 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00686 std::cerr << "\t ... Can't find attribute 'k1' of tag 'DistortionCoeffs '." << std::endl;
00687 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00688 }
00689 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "k2", &k2 ) != TIXML_SUCCESS)
00690 {
00691 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00692 std::cerr << "\t ... Can't find attribute 'k2' of tag 'DistortionCoeffs '." << std::endl;
00693 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00694 }
00695 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "p1", &p1 ) != TIXML_SUCCESS)
00696 {
00697 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00698 std::cerr << "\t ... Can't find attribute 'p1' of tag 'DistortionCoeffs '." << std::endl;
00699 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00700 }
00701 if ( p_xmlElement_Intrinsics_Child->QueryValueAttribute( "p2", &p2 ) != TIXML_SUCCESS)
00702 {
00703 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00704 std::cerr << "\t ... Can't find attribute 'p2' of tag 'DistortionCoeffs '." << std::endl;
00705 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00706 }
00707
00708 distortionCoeffs.at<double>(0, 0) = k1;
00709 distortionCoeffs.at<double>(1, 0) = k2;
00710 distortionCoeffs.at<double>(2, 0) = p1;
00711 distortionCoeffs.at<double>(3, 0) = p2;
00712 }
00713 else
00714 {
00715 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00716 std::cerr << "\t ... Can't find tag 'DistortionCoeffs '." << std::endl;
00717 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00718 }
00719
00720 SetIntrinsicParameters(p_xmlElement_Intrinsics->Value(), intrinsicMatrix, distortionCoeffs);
00721 }
00722 }
00723 else
00724 {
00725 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00726 std::cerr << "\t ... Can't find tag 'IntrinsicParameters'." << std::endl;
00727 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00728 }
00729
00730
00731
00732
00733
00734 p_xmlElement_Child = NULL;
00735 p_xmlElement_Child = p_xmlElement_Root_Cam->FirstChildElement( "ExtrinsicParameters" );
00736 if ( p_xmlElement_Child )
00737 {
00738 TiXmlElement *p_xmlElement_Extrinsics = 0;
00739 TiXmlElement *p_xmlElement_Extrinsics_Child = 0;
00740
00741 for( p_xmlElement_Extrinsics = p_xmlElement_Child->FirstChildElement();
00742 p_xmlElement_Extrinsics;
00743 p_xmlElement_Extrinsics = p_xmlElement_Extrinsics->NextSiblingElement())
00744 {
00745
00746
00747
00748
00749
00750 cv::Mat extrinsicTranslation (3, 1, CV_64FC(1));
00751 p_xmlElement_Extrinsics_Child = NULL;
00752 p_xmlElement_Extrinsics_Child = p_xmlElement_Extrinsics->FirstChildElement( "Translation" );
00753 if ( p_xmlElement_Extrinsics_Child )
00754 {
00755 double x, y, z;
00756
00757 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x", &x ) != TIXML_SUCCESS)
00758 {
00759 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00760 std::cerr << "\t ... Can't find attribute 'x' of tag 'Translation'." << std::endl;
00761 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00762 }
00763 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "y", &y ) != TIXML_SUCCESS)
00764 {
00765 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00766 std::cerr << "\t ... Can't find attribute 'y' of tag 'Translation'." << std::endl;
00767 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00768 }
00769 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "z", &z ) != TIXML_SUCCESS)
00770 {
00771 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00772 std::cerr << "\t ... Can't find attribute 'z' of tag 'Translation'." << std::endl;
00773 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00774 }
00775 extrinsicTranslation.at<double>(0, 0) = x;
00776 extrinsicTranslation.at<double>(1, 0) = y;
00777 extrinsicTranslation.at<double>(2, 0) = z;
00778 }
00779 else
00780 {
00781 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00782 std::cerr << "\t ... Can't find tag 'Translation'." << std::endl;
00783 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00784 }
00785
00786
00787
00788
00789
00790 cv::Mat extrinsicRotation (3, 3, CV_64FC(1));
00791 p_xmlElement_Extrinsics_Child = NULL;
00792 p_xmlElement_Extrinsics_Child = p_xmlElement_Extrinsics->FirstChildElement( "Rotation" );
00793 if ( p_xmlElement_Extrinsics_Child )
00794 {
00795 double x11, x12, x13;
00796 double x21, x22, x23;
00797 double x31, x32, x33;
00798
00799 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x11", &x11 ) != TIXML_SUCCESS)
00800 {
00801 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:R" << std::endl;
00802 std::cerr << "\t ... Can't find attribute 'x11' of tag 'Rotation'." << std::endl;
00803 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00804 }
00805 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x12", &x12 ) != TIXML_SUCCESS)
00806 {
00807 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00808 std::cerr << "\t ... Can't find attribute 'x12' of tag 'Rotation'." << std::endl;
00809 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00810 }
00811 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x13", &x13 ) != TIXML_SUCCESS)
00812 {
00813 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00814 std::cerr << "\t ... Can't find attribute 'x13' of tag 'Rotation'." << std::endl;
00815 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00816 }
00817 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x21", &x21 ) != TIXML_SUCCESS)
00818 {
00819 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00820 std::cerr << "\t ... Can't find attribute 'x21' of tag 'Rotation'." << std::endl;
00821 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00822 }
00823 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x22", &x22 ) != TIXML_SUCCESS)
00824 {
00825 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00826 std::cerr << "\t ... Can't find attribute 'x22' of tag 'Rotation'." << std::endl;
00827 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00828 }
00829 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x23", &x23 ) != TIXML_SUCCESS)
00830 {
00831 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00832 std::cerr << "\t ... Can't find attribute 'x23' of tag 'Rotation'." << std::endl;
00833 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00834 }
00835 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x31", &x31 ) != TIXML_SUCCESS)
00836 {
00837 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00838 std::cerr << "\t ... Can't find attribute 'x31' of tag 'Rotation'." << std::endl;
00839 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00840 }
00841 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x32", &x32 ) != TIXML_SUCCESS)
00842 {
00843 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00844 std::cerr << "\t ... Can't find attribute 'x32' of tag 'Rotation'." << std::endl;
00845 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00846 }
00847 if ( p_xmlElement_Extrinsics_Child->QueryValueAttribute( "x33", &x33 ) != TIXML_SUCCESS)
00848 {
00849 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00850 std::cerr << "\t ... Can't find attribute 'x33' of tag 'Rotation'." << std::endl;
00851 return (RET_FAILED | RET_XML_ATTR_NOT_FOUND);
00852 }
00853
00854 extrinsicRotation.at<double>(0, 0) = x11;
00855 extrinsicRotation.at<double>(0, 1) = x12;
00856 extrinsicRotation.at<double>(0, 2) = x13;
00857 extrinsicRotation.at<double>(1, 0) = x21;
00858 extrinsicRotation.at<double>(1, 1) = x22;
00859 extrinsicRotation.at<double>(1, 2) = x23;
00860 extrinsicRotation.at<double>(2, 0) = x31;
00861 extrinsicRotation.at<double>(2, 1) = x32;
00862 extrinsicRotation.at<double>(2, 2) = x33;
00863 }
00864 else
00865 {
00866 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00867 std::cerr << "\t ... Can't find tag 'Rotation'." << std::endl;
00868 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00869 }
00870
00871 SetExtrinsicParameters(p_xmlElement_Extrinsics->Value(), extrinsicRotation, extrinsicTranslation);
00872 }
00873 }
00874 else
00875 {
00876 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00877 std::cerr << "\t ... Can't find tag 'ExtrinsicParameters'." << std::endl;
00878 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00879 }
00880 }
00881
00882
00883
00884
00885 else
00886 {
00887 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00888 std::cerr << "\t ... Can't find tag '" << ss.str() << "'" << std::endl;
00889 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00890 }
00891 }
00892
00893
00894
00895
00896 else
00897 {
00898 std::cerr << "ERROR - CameraSensorsToolbox::LoadParameters:" << std::endl;
00899 std::cerr << "\t ... Can't find tag 'LibCameraSensors'." << std::endl;
00900 return (RET_FAILED | RET_XML_TAG_NOT_FOUND);
00901 }
00902 }
00903
00904 return RET_OK;
00905 }