$search
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: Toolbox for cameras. 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: July 2009 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 00059 00060 #ifndef __IPA_CAMERASENSORTOOLBOX_H__ 00061 #define __IPA_CAMERASENSORTOOLBOX_H__ 00062 00063 //#include "../../../../../cob_object_perception_intern/windows/src/PreCompiledHeaders/StdAfx.h" 00064 00065 #ifdef __LINUX__ 00066 #include "cob_vision_utils/CameraSensorDefines.h" 00067 #include "cob_vision_utils/CameraSensorTypes.h" 00068 #else 00069 #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/CameraSensorDefines.h" 00070 #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/CameraSensorTypes.h" 00071 #endif 00072 00073 #include <opencv/cv.h> 00074 #include <map> 00075 #include <boost/shared_ptr.hpp> 00076 00077 namespace ipa_CameraSensors { 00078 00080 class CameraSensorToolbox; 00081 typedef boost::shared_ptr<CameraSensorToolbox> CameraSensorToolboxPtr; 00082 00087 class __DLL_LIBCAMERASENSORS__ CameraSensorToolbox 00088 { 00089 public: 00090 CameraSensorToolbox(); 00091 ~CameraSensorToolbox(); 00092 00093 CameraSensorToolbox(const CameraSensorToolbox& cameraSensorToolbox); 00094 00096 CameraSensorToolbox& operator= (const CameraSensorToolbox& cameraSensorToolbox); 00097 00100 virtual unsigned long Release(); 00101 00109 virtual unsigned long Init(std::string directory, ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, const CvSize imageSize); 00110 00120 virtual unsigned long Init(const std::map<std::string, cv::Mat>* intrinsicMatrices, 00121 const std::map<std::string, cv::Mat>* distortionParameters, 00122 const std::map<std::string, cv::Mat>* extrinsicMatrices, 00123 const std::map<std::string, cv::Mat>* undistortMapsX, 00124 const std::map<std::string, cv::Mat>* undistortMapY, 00125 const CvSize imageSize); 00126 00133 virtual cv::Mat GetExtrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00134 00143 virtual unsigned long SetExtrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, 00144 const cv::Mat& _rotation, const cv::Mat& _translation); 00145 00153 virtual unsigned long SetExtrinsicParameters(std::string key, 00154 const cv::Mat& _rotation, const cv::Mat& _translation); 00155 00160 virtual cv::Mat GetIntrinsicMatrix(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00161 00168 virtual cv::Mat GetDistortionParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00169 00175 virtual cv::Mat GetDistortionMapY(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00176 00182 virtual cv::Mat GetDistortionMapX(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00183 00195 virtual unsigned long SetIntrinsicParameters(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, 00196 const cv::Mat& _intrinsicMatrix, const cv::Mat& _distortion_coeffs); 00197 00208 virtual unsigned long SetIntrinsicParameters(std::string key, const cv::Mat& _intrinsicMatrix, const cv::Mat& _distortion_coeffs); 00209 00217 virtual unsigned long RemoveDistortion(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, 00218 const cv::Mat& src, cv::Mat& dst); 00219 00230 virtual unsigned long ReprojectXYZ(ipa_CameraSensors::t_cameraType cameraType, int cameraIndex, 00231 double x, double y, double z, int& u, int& v); 00232 00233 private: 00234 00239 virtual unsigned long ConvertCameraTypeToString(ipa_CameraSensors::t_cameraType cameraType, std::string &cameraTypeString); 00240 00247 virtual unsigned long LoadParameters(const char* filename, ipa_CameraSensors::t_cameraType cameraType, int cameraIndex); 00248 00249 bool m_Initialized; 00250 00251 std::map<std::string, cv::Mat> m_intrinsicMatrices; 00252 std::map<std::string, cv::Mat> m_distortionCoeffs; 00253 std::map<std::string, cv::Mat> m_extrinsicMatrices; 00254 00255 00256 std::map<std::string, cv::Mat> m_undistortMapsX; 00257 std::map<std::string, cv::Mat> m_undistortMapsY; 00258 00259 CvSize m_ImageSize; 00260 }; 00261 00264 __DLL_LIBCAMERASENSORS__ CameraSensorToolboxPtr CreateCameraSensorToolbox(); 00265 00266 } // end namespace 00267 #endif // __IPA_CAMERASENSORTOOLBOX_H__