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