cob_vision_utils: cob_vision_utils: CameraSensorToolbox.cpp Source File

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