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


cob_vision_utils
Author(s): Jan Fischer
autogenerated on Fri Mar 15 2019 03:10:16