├── C++_ROS_hydro ├── C++_ROS_hydro_README.pdf ├── camera_left.yaml ├── src │ ├── defraction_map_finder │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── defraction_map_finder │ │ │ │ ├── Camera.h │ │ │ │ ├── CameraFactory.h │ │ │ │ ├── CataCamera.h │ │ │ │ ├── EquidistantCamera.h │ │ │ │ ├── MapFinder.hpp │ │ │ │ └── PinholeCamera.h │ │ ├── package.xml │ │ └── src │ │ │ ├── Camera.cc │ │ │ ├── CameraFactory.cc │ │ │ ├── CataCamera.cc │ │ │ ├── EquidistantCamera.cc │ │ │ ├── MapFinder.cpp │ │ │ ├── PinholeCamera.cc │ │ │ └── defraction_map_finder.cpp │ ├── jir_image_remapper │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── image_remapper.launch │ │ ├── package.xml │ │ └── src │ │ │ └── node.cpp │ ├── jir_rectification_remap_lib │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── jir_rectification_remap_lib │ │ │ │ └── rectification_remapper.h │ │ ├── package.xml │ │ └── src │ │ │ └── rectification_remapper.cpp │ ├── jir_refractive_image_geometry │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── jir_refractive_image_geometry │ │ │ │ ├── pinhole_camera_model.hpp │ │ │ │ ├── polynomial.hpp │ │ │ │ ├── polynomial_roots_gsl.hpp │ │ │ │ ├── ray.hpp │ │ │ │ ├── ray_operations.hpp │ │ │ │ ├── refracted_pinhole_camera_model.hpp │ │ │ │ ├── refraction_operations.hpp │ │ │ │ └── two_layer_refraction.hpp │ │ └── package.xml │ └── jir_refractive_image_geometry_msgs │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── jir_refractive_image_geometry_msgs │ │ │ └── helpers.hpp │ │ ├── msg │ │ └── PlanarRefractionInfo.msg │ │ └── package.xml └── testBag.bag ├── MATLAB ├── Find_correction_map │ ├── FindMap.m │ ├── RayTrace.m │ ├── RefractedRay.m │ ├── SolveForwardProjectionCase3.m │ └── testImg.jpg ├── MATLAB_README.pdf └── Optimal_d_0 │ ├── Main.m │ ├── RayTrace.m │ └── optim_d_0.m └── README_LICENSE /C++_ROS_hydro/C++_ROS_hydro_README.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomluc/Pinax-camera-model/e57b11918ada9c674c83786d54a61a3e86fd5419/C++_ROS_hydro/C++_ROS_hydro_README.pdf -------------------------------------------------------------------------------- /C++_ROS_hydro/camera_left.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | model_type: KANNALA_BRANDT 3 | camera_name: camera_left 4 | image_width: 1024 5 | image_height: 768 6 | projection_parameters: 7 | k2: -6.1370454578352829e-02 8 | k3: 6.9425360935635935e-03 9 | k4: -1.3696817533538066e-02 10 | k5: 7.5104868785324163e-03 11 | mu: 5.4459522674724587e+02 12 | mv: 5.4459958938156240e+02 13 | u0: 5.0280837875975055e+02 14 | v0: 3.9648539008190340e+02 15 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(defraction_map_finder) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | jir_refractive_image_geometry 9 | roscpp 10 | rospy 11 | std_msgs 12 | cmake_modules 13 | ) 14 | 15 | set(CMAKE_BUILD_TYPE Debug) 16 | 17 | find_package(Eigen REQUIRED) 18 | find_package(OpenCV REQUIRED) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | find_package(Boost REQUIRED COMPONENTS program_options) 22 | 23 | find_package(OpenMP) 24 | if (OPENMP_FOUND) 25 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 26 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 27 | endif() 28 | 29 | ## Uncomment this if the package has a setup.py. This macro ensures 30 | ## modules and global scripts declared therein get installed 31 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 32 | # catkin_python_setup() 33 | 34 | ################################################ 35 | ## Declare ROS messages, services and actions ## 36 | ################################################ 37 | 38 | ## To declare and build messages, services or actions from within this 39 | ## package, follow these steps: 40 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 41 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 42 | ## * In the file package.xml: 43 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 45 | ## pulled in transitively but can be declared for certainty nonetheless: 46 | ## * add a build_depend tag for "message_generation" 47 | ## * add a run_depend tag for "message_runtime" 48 | ## * In this file (CMakeLists.txt): 49 | ## * add "message_generation" and every package in MSG_DEP_SET to 50 | ## find_package(catkin REQUIRED COMPONENTS ...) 51 | ## * add "message_runtime" and every package in MSG_DEP_SET to 52 | ## catkin_package(CATKIN_DEPENDS ...) 53 | ## * uncomment the add_*_files sections below as needed 54 | ## and list every .msg/.srv/.action file to be processed 55 | ## * uncomment the generate_messages entry below 56 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 57 | 58 | ## Generate messages in the 'msg' folder 59 | # add_message_files( 60 | # FILES 61 | # Message1.msg 62 | # Message2.msg 63 | # ) 64 | 65 | ## Generate services in the 'srv' folder 66 | # add_service_files( 67 | # FILES 68 | # Service1.srv 69 | # Service2.srv 70 | # ) 71 | 72 | ## Generate actions in the 'action' folder 73 | # add_action_files( 74 | # FILES 75 | # Action1.action 76 | # Action2.action 77 | # ) 78 | 79 | ## Generate added messages and services with any dependencies listed here 80 | # generate_messages( 81 | # DEPENDENCIES 82 | # std_msgs 83 | # ) 84 | 85 | ################################### 86 | ## catkin specific configuration ## 87 | ################################### 88 | ## The catkin_package macro generates cmake config files for your package 89 | ## Declare things to be passed to dependent projects 90 | ## INCLUDE_DIRS: uncomment this if you package contains header files 91 | ## LIBRARIES: libraries you create in this project that dependent projects also need 92 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 93 | ## DEPENDS: system dependencies of this project that dependent projects also need 94 | catkin_package( 95 | INCLUDE_DIRS include 96 | # LIBRARIES defraction_map_finder 97 | CATKIN_DEPENDS jir_refractive_image_geometry roscpp rospy std_msgs 98 | DEPENDS Eigen 99 | ) 100 | 101 | ########### 102 | ## Build ## 103 | ########### 104 | 105 | ## Specify additional locations of header files 106 | ## Your package locations should be listed before other locations 107 | include_directories(include) 108 | include_directories( 109 | ${catkin_INCLUDE_DIRS} 110 | ${OpenCV_INCLUDE_DIRS} 111 | ${Eigen_INCLUDE_DIRS} 112 | ${Boost_INCLUDE_DIRS} 113 | ) 114 | 115 | 116 | ## Declare a cpp library 117 | # add_library(defraction_map_finder 118 | # src/${PROJECT_NAME}/defraction_map_finder.cpp 119 | # ) 120 | 121 | ## Declare a cpp executable 122 | add_executable(defraction_map_finder src/defraction_map_finder.cpp src/MapFinder.cpp src/Camera.cc src/CameraFactory.cc src/CataCamera.cc src/EquidistantCamera.cc src/PinholeCamera.cc) 123 | 124 | ## Add cmake target dependencies of the executable/library 125 | ## as an example, message headers may need to be generated before nodes 126 | # add_dependencies(defraction_map_finder_node defraction_map_finder_generate_messages_cpp) 127 | 128 | ## Specify libraries to link a library or executable target against 129 | target_link_libraries(defraction_map_finder 130 | ${catkin_LIBRARIES} 131 | ${OpenCV_LIBRARIES} 132 | ) 133 | 134 | 135 | 136 | ############# 137 | ## Install ## 138 | ############# 139 | 140 | # all install targets should use catkin DESTINATION variables 141 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 142 | 143 | ## Mark executable scripts (Python etc.) for installation 144 | ## in contrast to setup.py, you can choose the destination 145 | # install(PROGRAMS 146 | # scripts/my_python_script 147 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 148 | # ) 149 | 150 | ## Mark executables and/or libraries for installation 151 | # install(TARGETS defraction_map_finder defraction_map_finder_node 152 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 154 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 155 | # ) 156 | 157 | ## Mark cpp header files for installation 158 | # install(DIRECTORY include/${PROJECT_NAME}/ 159 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 160 | # FILES_MATCHING PATTERN "*.h" 161 | # PATTERN ".svn" EXCLUDE 162 | # ) 163 | 164 | ## Mark other files for installation (e.g. launch and bag files, etc.) 165 | # install(FILES 166 | # # myfile1 167 | # # myfile2 168 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 169 | # ) 170 | 171 | ############# 172 | ## Testing ## 173 | ############# 174 | 175 | ## Add gtest based cpp test target and link libraries 176 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_defraction_map_finder.cpp) 177 | # if(TARGET ${PROJECT_NAME}-test) 178 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 179 | # endif() 180 | 181 | ## Add folders to be run by python nosetests 182 | # catkin_add_nosetests(test) 183 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/Camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #ifndef CAMERA_H 17 | #define CAMERA_H 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace camodocal 25 | { 26 | 27 | class Camera 28 | { 29 | public: 30 | enum ModelType 31 | { 32 | KANNALA_BRANDT, 33 | MEI, 34 | PINHOLE 35 | }; 36 | 37 | class Parameters 38 | { 39 | public: 40 | Parameters(ModelType modelType); 41 | 42 | Parameters(ModelType modelType, const std::string& cameraName, 43 | int w, int h); 44 | 45 | ModelType& modelType(void); 46 | std::string& cameraName(void); 47 | int& imageWidth(void); 48 | int& imageHeight(void); 49 | 50 | ModelType modelType(void) const; 51 | const std::string& cameraName(void) const; 52 | int imageWidth(void) const; 53 | int imageHeight(void) const; 54 | 55 | int nIntrinsics(void) const; 56 | 57 | virtual bool readFromYamlFile(const std::string& filename) = 0; 58 | virtual void writeToYamlFile(const std::string& filename) const = 0; 59 | 60 | protected: 61 | ModelType m_modelType; 62 | int m_nIntrinsics; 63 | std::string m_cameraName; 64 | int m_imageWidth; 65 | int m_imageHeight; 66 | }; 67 | 68 | virtual ModelType modelType(void) const = 0; 69 | virtual const std::string& cameraName(void) const = 0; 70 | virtual int imageWidth(void) const = 0; 71 | virtual int imageHeight(void) const = 0; 72 | 73 | /* virtual cv::Mat& mask(void); 74 | virtual const cv::Mat& mask(void) const; 75 | 76 | virtual void estimateIntrinsics(const cv::Size& boardSize, 77 | const std::vector< std::vector >& objectPoints, 78 | const std::vector< std::vector >& imagePoints) = 0; 79 | virtual void estimateExtrinsics(const std::vector& objectPoints, 80 | const std::vector& imagePoints, 81 | cv::Mat& rvec, cv::Mat& tvec) const; 82 | 83 | // Lift points from the image plane to the sphere 84 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 85 | //%output P 86 | 87 | // Lift points from the image plane to the projective space 88 | virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 89 | //%output P 90 | */ 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; 94 | //%output p 95 | /* 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const = 0; 100 | //%output p 101 | //%output J 102 | 103 | virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; 104 | //%output p 105 | 106 | virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; 107 | virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 108 | float fx = -1.0f, float fy = -1.0f, 109 | cv::Size imageSize = cv::Size(0, 0), 110 | float cx = -1.0f, float cy = -1.0f, 111 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; 112 | 113 | virtual int parameterCount(void) const = 0; 114 | 115 | virtual void readParameters(const std::vector& parameters) = 0; 116 | virtual void writeParameters(std::vector& parameters) const = 0; 117 | 118 | virtual void writeParametersToYamlFile(const std::string& filename) const = 0; 119 | 120 | virtual std::string parametersToString(void) const = 0; 121 | */ 122 | /** 123 | * \brief Calculates the reprojection distance between points 124 | * 125 | * \param P1 first 3D point coordinates 126 | * \param P2 second 3D point coordinates 127 | * \return euclidean distance in the plane 128 | */ 129 | double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; 130 | 131 | double reprojectionError(const std::vector< std::vector >& objectPoints, 132 | const std::vector< std::vector >& imagePoints, 133 | const std::vector& rvecs, 134 | const std::vector& tvecs, 135 | cv::OutputArray perViewErrors = cv::noArray()) const; 136 | 137 | double reprojectionError(const Eigen::Vector3d& P, 138 | const Eigen::Quaterniond& camera_q, 139 | const Eigen::Vector3d& camera_t, 140 | const Eigen::Vector2d& observed_p) const; 141 | 142 | void projectPoints(const std::vector& objectPoints, 143 | const cv::Mat& rvec, 144 | const cv::Mat& tvec, 145 | std::vector& imagePoints) const; 146 | protected: 147 | cv::Mat m_mask; 148 | }; 149 | 150 | typedef boost::shared_ptr CameraPtr; 151 | typedef boost::shared_ptr CameraConstPtr; 152 | 153 | } 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/CameraFactory.h: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #ifndef CAMERAFACTORY_H 17 | #define CAMERAFACTORY_H 18 | 19 | #include 20 | #include 21 | 22 | #include "defraction_map_finder/Camera.h" 23 | 24 | namespace camodocal 25 | { 26 | 27 | class CameraFactory 28 | { 29 | public: 30 | CameraFactory(); 31 | 32 | static boost::shared_ptr instance(void); 33 | 34 | CameraPtr generateCamera(Camera::ModelType modelType, 35 | const std::string& cameraName, 36 | cv::Size imageSize) const; 37 | 38 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 39 | 40 | private: 41 | static boost::shared_ptr m_instance; 42 | }; 43 | 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/CataCamera.h: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #ifndef CATACAMERA_H 17 | #define CATACAMERA_H 18 | 19 | #include 20 | #include 21 | 22 | //#include "ceres/rotation.h" 23 | #include "Camera.h" 24 | 25 | namespace camodocal 26 | { 27 | 28 | /** 29 | * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration 30 | * from Planar Grids, ICRA 2007 31 | */ 32 | 33 | class CataCamera: public Camera 34 | { 35 | public: 36 | class Parameters: public Camera::Parameters 37 | { 38 | public: 39 | Parameters(); 40 | Parameters(const std::string& cameraName, 41 | int w, int h, 42 | double xi, 43 | double k1, double k2, double p1, double p2, 44 | double gamma1, double gamma2, double u0, double v0); 45 | 46 | double& xi(void); 47 | double& k1(void); 48 | double& k2(void); 49 | double& p1(void); 50 | double& p2(void); 51 | double& gamma1(void); 52 | double& gamma2(void); 53 | double& u0(void); 54 | double& v0(void); 55 | 56 | double xi(void) const; 57 | double k1(void) const; 58 | double k2(void) const; 59 | double p1(void) const; 60 | double p2(void) const; 61 | double gamma1(void) const; 62 | double gamma2(void) const; 63 | double u0(void) const; 64 | double v0(void) const; 65 | 66 | bool readFromYamlFile(const std::string& filename); 67 | void writeToYamlFile(const std::string& filename) const; 68 | 69 | Parameters& operator=(const Parameters& other); 70 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 71 | 72 | private: 73 | double m_xi; 74 | double m_k1; 75 | double m_k2; 76 | double m_p1; 77 | double m_p2; 78 | double m_gamma1; 79 | double m_gamma2; 80 | double m_u0; 81 | double m_v0; 82 | }; 83 | 84 | CataCamera(); 85 | 86 | /** 87 | * \brief Constructor from the projection model parameters 88 | */ 89 | CataCamera(const std::string& cameraName, 90 | int imageWidth, int imageHeight, 91 | double xi, double k1, double k2, double p1, double p2, 92 | double gamma1, double gamma2, double u0, double v0); 93 | /** 94 | * \brief Constructor from the projection model parameters 95 | */ 96 | CataCamera(const Parameters& params); 97 | 98 | Camera::ModelType modelType(void) const; 99 | const std::string& cameraName(void) const; 100 | int imageWidth(void) const; 101 | int imageHeight(void) const; 102 | 103 | /*void estimateIntrinsics(const cv::Size& boardSize, 104 | const std::vector< std::vector >& objectPoints, 105 | const std::vector< std::vector >& imagePoints);*/ 106 | 107 | // Lift points from the image plane to the sphere 108 | // void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 109 | //%output P 110 | 111 | // Lift points from the image plane to the projective space 112 | // void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 113 | //%output P 114 | 115 | // Projects 3D points to the image plane (Pi function) 116 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 117 | //%output p 118 | 119 | // Projects 3D points to the image plane (Pi function) 120 | // and calculates jacobian 121 | /* void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 122 | Eigen::Matrix& J) const;*/ 123 | //%output p 124 | //%output J 125 | 126 | //void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 127 | //%output p 128 | 129 | /* template 130 | static void spaceToPlane(const T* const params, 131 | const T* const q, const T* const t, 132 | const Eigen::Matrix& P, 133 | Eigen::Matrix& p);*/ 134 | 135 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 136 | /* void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 137 | Eigen::Matrix2d& J) const; 138 | 139 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 140 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 141 | float fx = -1.0f, float fy = -1.0f, 142 | cv::Size imageSize = cv::Size(0, 0), 143 | float cx = -1.0f, float cy = -1.0f, 144 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 145 | 146 | int parameterCount(void) const; 147 | */ 148 | const Parameters& getParameters(void) const; 149 | void setParameters(const Parameters& parameters); 150 | /* 151 | void readParameters(const std::vector& parameterVec); 152 | void writeParameters(std::vector& parameterVec) const; 153 | 154 | void writeParametersToYamlFile(const std::string& filename) const; 155 | 156 | std::string parametersToString(void) const;*/ 157 | 158 | private: 159 | Parameters mParameters; 160 | 161 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 162 | bool m_noDistortion; 163 | }; 164 | 165 | typedef boost::shared_ptr CataCameraPtr; 166 | typedef boost::shared_ptr CataCameraConstPtr; 167 | 168 | /*template 169 | void 170 | CataCamera::spaceToPlane(const T* const params, 171 | const T* const q, const T* const t, 172 | const Eigen::Matrix& P, 173 | Eigen::Matrix& p) 174 | { 175 | T P_w[3]; 176 | P_w[0] = T(P(0)); 177 | P_w[1] = T(P(1)); 178 | P_w[2] = T(P(2)); 179 | 180 | // Convert quaternion from Eigen convention (x, y, z, w) 181 | // to Ceres convention (w, x, y, z) 182 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 183 | 184 | T P_c[3]; 185 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 186 | 187 | P_c[0] += t[0]; 188 | P_c[1] += t[1]; 189 | P_c[2] += t[2]; 190 | 191 | // project 3D object point to the image plane 192 | T xi = params[0]; 193 | T k1 = params[1]; 194 | T k2 = params[2]; 195 | T p1 = params[3]; 196 | T p2 = params[4]; 197 | T gamma1 = params[5]; 198 | T gamma2 = params[6]; 199 | T alpha = T(0); //cameraParams.alpha(); 200 | T u0 = params[7]; 201 | T v0 = params[8]; 202 | 203 | // Transform to model plane 204 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 205 | P_c[0] /= len; 206 | P_c[1] /= len; 207 | P_c[2] /= len; 208 | 209 | T u = P_c[0] / (P_c[2] + xi); 210 | T v = P_c[1] / (P_c[2] + xi); 211 | 212 | T rho_sqr = u * u + v * v; 213 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 214 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 215 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 216 | 217 | u = L * u + du; 218 | v = L * v + dv; 219 | p(0) = gamma1 * (u + alpha * v) + u0; 220 | p(1) = gamma2 * v + v0; 221 | }*/ 222 | 223 | } 224 | 225 | #endif 226 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/EquidistantCamera.h: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #ifndef EQUIDISTANTCAMERA_H 17 | #define EQUIDISTANTCAMERA_H 18 | 19 | #include 20 | #include 21 | 22 | #include "Camera.h" 23 | 24 | namespace camodocal 25 | { 26 | 27 | /** 28 | * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method 29 | * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 30 | */ 31 | 32 | class EquidistantCamera: public Camera 33 | { 34 | public: 35 | class Parameters: public Camera::Parameters 36 | { 37 | public: 38 | Parameters(); 39 | Parameters(const std::string& cameraName, 40 | int w, int h, 41 | double k2, double k3, double k4, double k5, 42 | double mu, double mv, 43 | double u0, double v0); 44 | 45 | double& k2(void); 46 | double& k3(void); 47 | double& k4(void); 48 | double& k5(void); 49 | double& mu(void); 50 | double& mv(void); 51 | double& u0(void); 52 | double& v0(void); 53 | 54 | double k2(void) const; 55 | double k3(void) const; 56 | double k4(void) const; 57 | double k5(void) const; 58 | double mu(void) const; 59 | double mv(void) const; 60 | double u0(void) const; 61 | double v0(void) const; 62 | 63 | bool readFromYamlFile(const std::string& filename); 64 | void writeToYamlFile(const std::string& filename) const; 65 | 66 | Parameters& operator=(const Parameters& other); 67 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 68 | 69 | private: 70 | // projection 71 | double m_k2; 72 | double m_k3; 73 | double m_k4; 74 | double m_k5; 75 | 76 | double m_mu; 77 | double m_mv; 78 | double m_u0; 79 | double m_v0; 80 | }; 81 | 82 | EquidistantCamera(); 83 | 84 | /** 85 | * \brief Constructor from the projection model parameters 86 | */ 87 | EquidistantCamera(const std::string& cameraName, 88 | int imageWidth, int imageHeight, 89 | double k2, double k3, double k4, double k5, 90 | double mu, double mv, 91 | double u0, double v0); 92 | /** 93 | * \brief Constructor from the projection model parameters 94 | */ 95 | EquidistantCamera(const Parameters& params); 96 | 97 | Camera::ModelType modelType(void) const; 98 | const std::string& cameraName(void) const; 99 | int imageWidth(void) const; 100 | int imageHeight(void) const; 101 | 102 | 103 | // Lift points from the image plane to the sphere 104 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 105 | //%output P 106 | 107 | // Lift points from the image plane to the projective space 108 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 109 | //%output P 110 | 111 | // Projects 3D points to the image plane (Pi function) 112 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 113 | //%output p 114 | 115 | // Projects 3D points to the image plane (Pi function) 116 | // and calculates jacobian 117 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 118 | Eigen::Matrix& J) const; 119 | //%output p 120 | //%output J 121 | 122 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 123 | //%output p 124 | 125 | template 126 | static void spaceToPlane(const T* const params, 127 | const T* const q, const T* const t, 128 | const Eigen::Matrix& P, 129 | Eigen::Matrix& p); 130 | 131 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 132 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 133 | float fx = -1.0f, float fy = -1.0f, 134 | cv::Size imageSize = cv::Size(0, 0), 135 | float cx = -1.0f, float cy = -1.0f, 136 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 137 | 138 | int parameterCount(void) const; 139 | 140 | const Parameters& getParameters(void) const; 141 | void setParameters(const Parameters& parameters); 142 | 143 | void readParameters(const std::vector& parameterVec); 144 | void writeParameters(std::vector& parameterVec) const; 145 | 146 | void writeParametersToYamlFile(const std::string& filename) const; 147 | 148 | std::string parametersToString(void) const; 149 | 150 | private: 151 | template 152 | static T r(T k2, T k3, T k4, T k5, T theta); 153 | 154 | 155 | void fitOddPoly(const std::vector& x, const std::vector& y, 156 | int n, std::vector& coeffs) const; 157 | 158 | void backprojectSymmetric(const Eigen::Vector2d& p_u, 159 | double& theta, double& phi) const; 160 | 161 | Parameters mParameters; 162 | 163 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 164 | }; 165 | 166 | typedef boost::shared_ptr EquidistantCameraPtr; 167 | typedef boost::shared_ptr EquidistantCameraConstPtr; 168 | 169 | template 170 | T 171 | EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) 172 | { 173 | // k1 = 1 174 | return theta + 175 | k2 * theta * theta * theta + 176 | k3 * theta * theta * theta * theta * theta + 177 | k4 * theta * theta * theta * theta * theta * theta * theta + 178 | k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; 179 | } 180 | 181 | 182 | } 183 | 184 | #endif 185 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/MapFinder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "defraction_map_finder/CataCamera.h" 43 | 44 | 45 | 46 | using namespace std; 47 | using namespace cv; 48 | using namespace Eigen; 49 | using namespace jir_refractive_image_geometry; 50 | using namespace camodocal; 51 | 52 | class MapFinder { 53 | public: 54 | MapFinder(void){}; 55 | ~MapFinder(void){}; 56 | 57 | MapFinder(int W, int H, float d_0, float d0off, float d_1, float n_g, float n_w, 58 | const sensor_msgs::CameraInfo info,CameraPtr realCam); 59 | 60 | 61 | void init_setup (int W, int H, float d_0, float d0off, float d_1, float n_g, float n_w, 62 | const sensor_msgs::CameraInfo info,CameraPtr realCam); 63 | void generate3Dpoints(); 64 | void projectPoints(); 65 | void saveMaps (const std::string& filename); 66 | 67 | cv::Mat getMask() const; 68 | 69 | private: 70 | Point2d projectPoint (Vector3d point3D, bool notRefracted); 71 | int ImgWidth; 72 | int ImgHeight; 73 | Mat mapx; 74 | Mat mapy; 75 | jir_refractive_image_geometry::RefractedPinholeCameraModel VirtualCamera; 76 | jir_refractive_image_geometry::RefractedPinholeCameraModel RealCamera; 77 | 78 | float d0; 79 | float d0Virt_offset; 80 | float d1; 81 | float ng; 82 | float nw; 83 | 84 | vector points3D; 85 | jir_refractive_image_geometry_msgs::PlanarRefractionInfo RefractionInfo; 86 | sensor_msgs::CameraInfo camera_info; 87 | CameraPtr myCamera; 88 | }; 89 | 90 | 91 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/include/defraction_map_finder/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #ifndef PINHOLECAMERA_H 17 | #define PINHOLECAMERA_H 18 | 19 | #include 20 | #include 21 | 22 | //#include "ceres/rotation.h" 23 | #include "Camera.h" 24 | 25 | namespace camodocal 26 | { 27 | 28 | class PinholeCamera: public Camera 29 | { 30 | public: 31 | class Parameters: public Camera::Parameters 32 | { 33 | public: 34 | Parameters(); 35 | Parameters(const std::string& cameraName, 36 | int w, int h, 37 | double k1, double k2, double p1, double p2, 38 | double fx, double fy, double cx, double cy); 39 | 40 | double& k1(void); 41 | double& k2(void); 42 | double& p1(void); 43 | double& p2(void); 44 | double& fx(void); 45 | double& fy(void); 46 | double& cx(void); 47 | double& cy(void); 48 | 49 | double xi(void) const; 50 | double k1(void) const; 51 | double k2(void) const; 52 | double p1(void) const; 53 | double p2(void) const; 54 | double fx(void) const; 55 | double fy(void) const; 56 | double cx(void) const; 57 | double cy(void) const; 58 | 59 | bool readFromYamlFile(const std::string& filename); 60 | void writeToYamlFile(const std::string& filename) const; 61 | 62 | Parameters& operator=(const Parameters& other); 63 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 64 | 65 | private: 66 | double m_k1; 67 | double m_k2; 68 | double m_p1; 69 | double m_p2; 70 | double m_fx; 71 | double m_fy; 72 | double m_cx; 73 | double m_cy; 74 | }; 75 | 76 | PinholeCamera(); 77 | 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | PinholeCamera(const std::string& cameraName, 82 | int imageWidth, int imageHeight, 83 | double k1, double k2, double p1, double p2, 84 | double fx, double fy, double cx, double cy); 85 | /** 86 | * \brief Constructor from the projection model parameters 87 | */ 88 | PinholeCamera(const Parameters& params); 89 | 90 | Camera::ModelType modelType(void) const; 91 | const std::string& cameraName(void) const; 92 | int imageWidth(void) const; 93 | int imageHeight(void) const; 94 | 95 | /*void estimateIntrinsics(const cv::Size& boardSize, 96 | const std::vector< std::vector >& objectPoints, 97 | const std::vector< std::vector >& imagePoints);*/ 98 | 99 | // Lift points from the image plane to the sphere 100 | // virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 101 | //%output P 102 | 103 | // Lift points from the image plane to the projective space 104 | // void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 105 | //%output P 106 | 107 | // Projects 3D points to the image plane (Pi function) 108 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 109 | //%output p 110 | 111 | // Projects 3D points to the image plane (Pi function) 112 | // and calculates jacobian 113 | /* void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 114 | Eigen::Matrix& J) const;*/ 115 | //%output p 116 | //%output J 117 | 118 | // void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 119 | //%output p 120 | 121 | /* template 122 | static void spaceToPlane(const T* const params, 123 | const T* const q, const T* const t, 124 | const Eigen::Matrix& P, 125 | Eigen::Matrix& p);*/ 126 | 127 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 128 | /* void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 129 | Eigen::Matrix2d& J) const;*/ 130 | 131 | /* void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 132 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 133 | float fx = -1.0f, float fy = -1.0f, 134 | cv::Size imageSize = cv::Size(0, 0), 135 | float cx = -1.0f, float cy = -1.0f, 136 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 137 | 138 | int parameterCount(void) const;*/ 139 | 140 | const Parameters& getParameters(void) const; 141 | void setParameters(const Parameters& parameters); 142 | 143 | // void readParameters(const std::vector& parameterVec); 144 | // void writeParameters(std::vector& parameterVec) const; 145 | 146 | // void writeParametersToYamlFile(const std::string& filename) const; 147 | 148 | // std::string parametersToString(void) const; 149 | 150 | private: 151 | Parameters mParameters; 152 | 153 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 154 | bool m_noDistortion; 155 | }; 156 | 157 | typedef boost::shared_ptr PinholeCameraPtr; 158 | typedef boost::shared_ptr PinholeCameraConstPtr; 159 | /* 160 | template 161 | void 162 | PinholeCamera::spaceToPlane(const T* const params, 163 | const T* const q, const T* const t, 164 | const Eigen::Matrix& P, 165 | Eigen::Matrix& p) 166 | { 167 | T P_w[3]; 168 | P_w[0] = T(P(0)); 169 | P_w[1] = T(P(1)); 170 | P_w[2] = T(P(2)); 171 | 172 | // Convert quaternion from Eigen convention (x, y, z, w) 173 | // to Ceres convention (w, x, y, z) 174 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 175 | 176 | T P_c[3]; 177 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 178 | 179 | P_c[0] += t[0]; 180 | P_c[1] += t[1]; 181 | P_c[2] += t[2]; 182 | 183 | // project 3D object point to the image plane 184 | T k1 = params[0]; 185 | T k2 = params[1]; 186 | T p1 = params[2]; 187 | T p2 = params[3]; 188 | T fx = params[4]; 189 | T fy = params[5]; 190 | T alpha = T(0); //cameraParams.alpha(); 191 | T cx = params[6]; 192 | T cy = params[7]; 193 | 194 | // Transform to model plane 195 | T u = P_c[0] / P_c[2]; 196 | T v = P_c[1] / P_c[2]; 197 | 198 | T rho_sqr = u * u + v * v; 199 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 200 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 201 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 202 | 203 | u = L * u + du; 204 | v = L * v + dv; 205 | p(0) = fx * (u + alpha * v) + cx; 206 | p(1) = fy * v + cy; 207 | } 208 | */ 209 | } 210 | 211 | #endif 212 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | defraction_map_finder 4 | 0.0.0 5 | The defraction_map_finder package 6 | 7 | 8 | 9 | 10 | Jacobs Robotics 11 | 12 | 13 | 14 | 15 | 16 | CC BY-NC-ND 4.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | jir_refractive_image_geometry 44 | roscpp 45 | rospy 46 | std_msgs 47 | jir_refractive_image_geometry 48 | roscpp 49 | rospy 50 | std_msgs 51 | cmake_modules 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/Camera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #include "defraction_map_finder/Camera.h" 17 | 18 | #include 19 | 20 | namespace camodocal 21 | { 22 | 23 | Camera::Parameters::Parameters(ModelType modelType) 24 | : m_modelType(modelType) 25 | , m_imageWidth(0) 26 | , m_imageHeight(0) 27 | { 28 | switch (modelType) 29 | { 30 | case KANNALA_BRANDT: 31 | m_nIntrinsics = 8; 32 | break; 33 | case PINHOLE: 34 | m_nIntrinsics = 8; 35 | break; 36 | case MEI: 37 | default: 38 | m_nIntrinsics = 9; 39 | } 40 | } 41 | 42 | Camera::Parameters::Parameters(ModelType modelType, 43 | const std::string& cameraName, 44 | int w, int h) 45 | : m_modelType(modelType) 46 | , m_cameraName(cameraName) 47 | , m_imageWidth(w) 48 | , m_imageHeight(h) 49 | { 50 | switch (modelType) 51 | { 52 | case KANNALA_BRANDT: 53 | m_nIntrinsics = 8; 54 | break; 55 | case PINHOLE: 56 | m_nIntrinsics = 8; 57 | break; 58 | case MEI: 59 | default: 60 | m_nIntrinsics = 9; 61 | } 62 | } 63 | 64 | Camera::ModelType& 65 | Camera::Parameters::modelType(void) 66 | { 67 | return m_modelType; 68 | } 69 | 70 | std::string& 71 | Camera::Parameters::cameraName(void) 72 | { 73 | return m_cameraName; 74 | } 75 | 76 | int& 77 | Camera::Parameters::imageWidth(void) 78 | { 79 | return m_imageWidth; 80 | } 81 | 82 | int& 83 | Camera::Parameters::imageHeight(void) 84 | { 85 | return m_imageHeight; 86 | } 87 | 88 | Camera::ModelType 89 | Camera::Parameters::modelType(void) const 90 | { 91 | return m_modelType; 92 | } 93 | 94 | const std::string& 95 | Camera::Parameters::cameraName(void) const 96 | { 97 | return m_cameraName; 98 | } 99 | 100 | int 101 | Camera::Parameters::imageWidth(void) const 102 | { 103 | return m_imageWidth; 104 | } 105 | 106 | int 107 | Camera::Parameters::imageHeight(void) const 108 | { 109 | return m_imageHeight; 110 | } 111 | 112 | int 113 | Camera::Parameters::nIntrinsics(void) const 114 | { 115 | return m_nIntrinsics; 116 | } 117 | /* 118 | cv::Mat& 119 | Camera::mask(void) 120 | { 121 | return m_mask; 122 | } 123 | 124 | const cv::Mat& 125 | Camera::mask(void) const 126 | { 127 | return m_mask; 128 | } 129 | 130 | void 131 | Camera::estimateExtrinsics(const std::vector& objectPoints, 132 | const std::vector& imagePoints, 133 | cv::Mat& rvec, cv::Mat& tvec) const 134 | { 135 | std::vector Ms(imagePoints.size()); 136 | for (size_t i = 0; i < Ms.size(); ++i) 137 | { 138 | Eigen::Vector3d P; 139 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 140 | 141 | P /= P(2); 142 | 143 | Ms.at(i).x = P(0); 144 | Ms.at(i).y = P(1); 145 | } 146 | 147 | // assume unit focal length, zero principal point, and zero distortion 148 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 149 | } 150 | */ 151 | double 152 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 153 | { 154 | Eigen::Vector2d p1, p2; 155 | 156 | spaceToPlane(P1, p1); 157 | spaceToPlane(P2, p2); 158 | 159 | return (p1 - p2).norm(); 160 | } 161 | 162 | double 163 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 164 | const std::vector< std::vector >& imagePoints, 165 | const std::vector& rvecs, 166 | const std::vector& tvecs, 167 | cv::OutputArray _perViewErrors) const 168 | { 169 | int imageCount = objectPoints.size(); 170 | size_t pointsSoFar = 0; 171 | double totalErr = 0.0; 172 | 173 | bool computePerViewErrors = _perViewErrors.needed(); 174 | cv::Mat perViewErrors; 175 | if (computePerViewErrors) 176 | { 177 | _perViewErrors.create(imageCount, 1, CV_64F); 178 | perViewErrors = _perViewErrors.getMat(); 179 | } 180 | 181 | for (int i = 0; i < imageCount; ++i) 182 | { 183 | size_t pointCount = imagePoints.at(i).size(); 184 | 185 | pointsSoFar += pointCount; 186 | 187 | std::vector estImagePoints; 188 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 189 | estImagePoints); 190 | 191 | double err = 0.0; 192 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 193 | { 194 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 195 | } 196 | 197 | if (computePerViewErrors) 198 | { 199 | perViewErrors.at(i) = err / pointCount; 200 | } 201 | 202 | totalErr += err; 203 | } 204 | 205 | return totalErr / pointsSoFar; 206 | } 207 | 208 | double 209 | Camera::reprojectionError(const Eigen::Vector3d& P, 210 | const Eigen::Quaterniond& camera_q, 211 | const Eigen::Vector3d& camera_t, 212 | const Eigen::Vector2d& observed_p) const 213 | { 214 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 215 | 216 | Eigen::Vector2d p; 217 | spaceToPlane(P_cam, p); 218 | 219 | return (p - observed_p).norm(); 220 | } 221 | 222 | void 223 | Camera::projectPoints(const std::vector& objectPoints, 224 | const cv::Mat& rvec, 225 | const cv::Mat& tvec, 226 | std::vector& imagePoints) const 227 | { 228 | // project 3D object points to the image plane 229 | imagePoints.reserve(objectPoints.size()); 230 | 231 | //double 232 | cv::Mat R0; 233 | cv::Rodrigues(rvec, R0); 234 | 235 | Eigen::MatrixXd R(3,3); 236 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 237 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 238 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 239 | 240 | Eigen::Vector3d t; 241 | t << tvec.at(0), tvec.at(1), tvec.at(2); 242 | 243 | for (size_t i = 0; i < objectPoints.size(); ++i) 244 | { 245 | const cv::Point3f& objectPoint = objectPoints.at(i); 246 | 247 | // Rotate and translate 248 | Eigen::Vector3d P; 249 | P << objectPoint.x, objectPoint.y, objectPoint.z; 250 | 251 | P = R * P + t; 252 | 253 | Eigen::Vector2d p; 254 | spaceToPlane(P, p); 255 | 256 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 257 | } 258 | } 259 | 260 | } 261 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #include "defraction_map_finder/CameraFactory.h" 17 | 18 | #include 19 | 20 | //#include "ceres/ceres.h" 21 | #include "defraction_map_finder/CataCamera.h" 22 | #include "defraction_map_finder/EquidistantCamera.h" 23 | #include "defraction_map_finder/PinholeCamera.h" 24 | //#include "camodocal/camera_models/EquidistantCamera.h" 25 | //#include "camodocal/camera_models/PinholeCamera.h" 26 | 27 | namespace camodocal 28 | { 29 | 30 | boost::shared_ptr CameraFactory::m_instance; 31 | 32 | CameraFactory::CameraFactory() 33 | { 34 | 35 | } 36 | 37 | boost::shared_ptr 38 | CameraFactory::instance(void) 39 | { 40 | if (m_instance.get() == 0) 41 | { 42 | m_instance.reset(new CameraFactory); 43 | } 44 | 45 | return m_instance; 46 | } 47 | 48 | CameraPtr 49 | CameraFactory::generateCamera(Camera::ModelType modelType, 50 | const std::string& cameraName, 51 | cv::Size imageSize) const 52 | { 53 | switch (modelType) 54 | { 55 | case Camera::KANNALA_BRANDT: 56 | { 57 | EquidistantCameraPtr camera(new EquidistantCamera); 58 | 59 | EquidistantCamera::Parameters params = camera->getParameters(); 60 | params.cameraName() = cameraName; 61 | params.imageWidth() = imageSize.width; 62 | params.imageHeight() = imageSize.height; 63 | camera->setParameters(params); 64 | return camera; 65 | } 66 | case Camera::PINHOLE: 67 | { 68 | PinholeCameraPtr camera(new PinholeCamera); 69 | 70 | PinholeCamera::Parameters params = camera->getParameters(); 71 | params.cameraName() = cameraName; 72 | params.imageWidth() = imageSize.width; 73 | params.imageHeight() = imageSize.height; 74 | camera->setParameters(params); 75 | return camera; 76 | return CameraPtr(); 77 | } 78 | case Camera::MEI: 79 | { 80 | CataCameraPtr camera(new CataCamera); 81 | 82 | CataCamera::Parameters params = camera->getParameters(); 83 | params.cameraName() = cameraName; 84 | params.imageWidth() = imageSize.width; 85 | params.imageHeight() = imageSize.height; 86 | camera->setParameters(params); 87 | return camera; 88 | } 89 | } 90 | } 91 | 92 | CameraPtr 93 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 94 | { 95 | cv::FileStorage fs(filename, cv::FileStorage::READ); 96 | 97 | if (!fs.isOpened()) 98 | { 99 | return CameraPtr(); 100 | } 101 | 102 | Camera::ModelType modelType = Camera::MEI; 103 | if (!fs["model_type"].isNone()) 104 | { 105 | std::string sModelType; 106 | fs["model_type"] >> sModelType; 107 | 108 | if (boost::iequals(sModelType, "kannala_brandt")) 109 | { 110 | modelType = Camera::KANNALA_BRANDT; 111 | } 112 | else if (boost::iequals(sModelType, "mei")) 113 | { 114 | modelType = Camera::MEI; 115 | } 116 | else if (boost::iequals(sModelType, "pinhole")) 117 | { 118 | modelType = Camera::PINHOLE; 119 | } 120 | else 121 | { 122 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 123 | return CameraPtr(); 124 | } 125 | } 126 | 127 | switch (modelType) 128 | { 129 | case Camera::KANNALA_BRANDT: 130 | { 131 | EquidistantCameraPtr camera(new EquidistantCamera); 132 | 133 | EquidistantCamera::Parameters params = camera->getParameters(); 134 | params.readFromYamlFile(filename); 135 | camera->setParameters(params); 136 | return camera; 137 | } 138 | case Camera::PINHOLE: 139 | { 140 | PinholeCameraPtr camera(new PinholeCamera); 141 | 142 | PinholeCamera::Parameters params = camera->getParameters(); 143 | params.readFromYamlFile(filename); 144 | camera->setParameters(params); 145 | return camera; 146 | } 147 | case Camera::MEI: 148 | default: 149 | { 150 | CataCameraPtr camera(new CataCamera); 151 | 152 | CataCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | } 158 | 159 | return CameraPtr(); 160 | } 161 | 162 | } 163 | 164 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/EquidistantCamera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | namespace camodocal 29 | { 30 | 31 | EquidistantCamera::Parameters::Parameters() 32 | : Camera::Parameters(KANNALA_BRANDT) 33 | , m_k2(0.0) 34 | , m_k3(0.0) 35 | , m_k4(0.0) 36 | , m_k5(0.0) 37 | , m_mu(0.0) 38 | , m_mv(0.0) 39 | , m_u0(0.0) 40 | , m_v0(0.0) 41 | { 42 | 43 | } 44 | 45 | EquidistantCamera::Parameters::Parameters(const std::string& cameraName, 46 | int w, int h, 47 | double k2, double k3, double k4, double k5, 48 | double mu, double mv, 49 | double u0, double v0) 50 | : Camera::Parameters(KANNALA_BRANDT, cameraName, w, h) 51 | , m_k2(k2) 52 | , m_k3(k3) 53 | , m_k4(k4) 54 | , m_k5(k5) 55 | , m_mu(mu) 56 | , m_mv(mv) 57 | , m_u0(u0) 58 | , m_v0(v0) 59 | { 60 | 61 | } 62 | 63 | double& 64 | EquidistantCamera::Parameters::k2(void) 65 | { 66 | return m_k2; 67 | } 68 | 69 | double& 70 | EquidistantCamera::Parameters::k3(void) 71 | { 72 | return m_k3; 73 | } 74 | 75 | double& 76 | EquidistantCamera::Parameters::k4(void) 77 | { 78 | return m_k4; 79 | } 80 | 81 | double& 82 | EquidistantCamera::Parameters::k5(void) 83 | { 84 | return m_k5; 85 | } 86 | 87 | double& 88 | EquidistantCamera::Parameters::mu(void) 89 | { 90 | return m_mu; 91 | } 92 | 93 | double& 94 | EquidistantCamera::Parameters::mv(void) 95 | { 96 | return m_mv; 97 | } 98 | 99 | double& 100 | EquidistantCamera::Parameters::u0(void) 101 | { 102 | return m_u0; 103 | } 104 | 105 | double& 106 | EquidistantCamera::Parameters::v0(void) 107 | { 108 | return m_v0; 109 | } 110 | 111 | double 112 | EquidistantCamera::Parameters::k2(void) const 113 | { 114 | return m_k2; 115 | } 116 | 117 | double 118 | EquidistantCamera::Parameters::k3(void) const 119 | { 120 | return m_k3; 121 | } 122 | 123 | double 124 | EquidistantCamera::Parameters::k4(void) const 125 | { 126 | return m_k4; 127 | } 128 | 129 | double 130 | EquidistantCamera::Parameters::k5(void) const 131 | { 132 | return m_k5; 133 | } 134 | 135 | double 136 | EquidistantCamera::Parameters::mu(void) const 137 | { 138 | return m_mu; 139 | } 140 | 141 | double 142 | EquidistantCamera::Parameters::mv(void) const 143 | { 144 | return m_mv; 145 | } 146 | 147 | double 148 | EquidistantCamera::Parameters::u0(void) const 149 | { 150 | return m_u0; 151 | } 152 | 153 | double 154 | EquidistantCamera::Parameters::v0(void) const 155 | { 156 | return m_v0; 157 | } 158 | 159 | bool 160 | EquidistantCamera::Parameters::readFromYamlFile(const std::string& filename) 161 | { 162 | cv::FileStorage fs(filename, cv::FileStorage::READ); 163 | 164 | if (!fs.isOpened()) 165 | { 166 | return false; 167 | } 168 | 169 | if (!fs["model_type"].isNone()) 170 | { 171 | std::string sModelType; 172 | fs["model_type"] >> sModelType; 173 | 174 | if (sModelType.compare("KANNALA_BRANDT") != 0) 175 | { 176 | return false; 177 | } 178 | } 179 | 180 | m_modelType = KANNALA_BRANDT; 181 | fs["camera_name"] >> m_cameraName; 182 | m_imageWidth = static_cast(fs["image_width"]); 183 | m_imageHeight = static_cast(fs["image_height"]); 184 | 185 | cv::FileNode n = fs["projection_parameters"]; 186 | m_k2 = static_cast(n["k2"]); 187 | m_k3 = static_cast(n["k3"]); 188 | m_k4 = static_cast(n["k4"]); 189 | m_k5 = static_cast(n["k5"]); 190 | m_mu = static_cast(n["mu"]); 191 | m_mv = static_cast(n["mv"]); 192 | m_u0 = static_cast(n["u0"]); 193 | m_v0 = static_cast(n["v0"]); 194 | 195 | return true; 196 | } 197 | 198 | void 199 | EquidistantCamera::Parameters::writeToYamlFile(const std::string& filename) const 200 | { 201 | cv::FileStorage fs(filename, cv::FileStorage::WRITE); 202 | 203 | fs << "model_type" << "KANNALA_BRANDT"; 204 | fs << "camera_name" << m_cameraName; 205 | fs << "image_width" << m_imageWidth; 206 | fs << "image_height" << m_imageHeight; 207 | 208 | // projection: k2, k3, k4, k5, mu, mv, u0, v0 209 | fs << "projection_parameters"; 210 | fs << "{" << "k2" << m_k2 211 | << "k3" << m_k3 212 | << "k4" << m_k4 213 | << "k5" << m_k5 214 | << "mu" << m_mu 215 | << "mv" << m_mv 216 | << "u0" << m_u0 217 | << "v0" << m_v0 << "}"; 218 | 219 | fs.release(); 220 | } 221 | 222 | EquidistantCamera::Parameters& 223 | EquidistantCamera::Parameters::operator=(const EquidistantCamera::Parameters& other) 224 | { 225 | if (this != &other) 226 | { 227 | m_modelType = other.m_modelType; 228 | m_cameraName = other.m_cameraName; 229 | m_imageWidth = other.m_imageWidth; 230 | m_imageHeight = other.m_imageHeight; 231 | m_k2 = other.m_k2; 232 | m_k3 = other.m_k3; 233 | m_k4 = other.m_k4; 234 | m_k5 = other.m_k5; 235 | m_mu = other.m_mu; 236 | m_mv = other.m_mv; 237 | m_u0 = other.m_u0; 238 | m_v0 = other.m_v0; 239 | } 240 | 241 | return *this; 242 | } 243 | 244 | std::ostream& 245 | operator<< (std::ostream& out, const EquidistantCamera::Parameters& params) 246 | { 247 | out << "Camera Parameters:" << std::endl; 248 | out << " model_type " << "KANNALA_BRANDT" << std::endl; 249 | out << " camera_name " << params.m_cameraName << std::endl; 250 | out << " image_width " << params.m_imageWidth << std::endl; 251 | out << " image_height " << params.m_imageHeight << std::endl; 252 | 253 | // projection: k2, k3, k4, k5, mu, mv, u0, v0 254 | out << "Projection Parameters" << std::endl; 255 | out << " k2 " << params.m_k2 << std::endl 256 | << " k3 " << params.m_k3 << std::endl 257 | << " k4 " << params.m_k4 << std::endl 258 | << " k5 " << params.m_k5 << std::endl 259 | << " mu " << params.m_mu << std::endl 260 | << " mv " << params.m_mv << std::endl 261 | << " u0 " << params.m_u0 << std::endl 262 | << " v0 " << params.m_v0 << std::endl; 263 | 264 | return out; 265 | } 266 | 267 | EquidistantCamera::EquidistantCamera() 268 | : m_inv_K11(1.0) 269 | , m_inv_K13(0.0) 270 | , m_inv_K22(1.0) 271 | , m_inv_K23(0.0) 272 | { 273 | 274 | } 275 | 276 | EquidistantCamera::EquidistantCamera(const std::string& cameraName, 277 | int imageWidth, int imageHeight, 278 | double k2, double k3, double k4, double k5, 279 | double mu, double mv, 280 | double u0, double v0) 281 | : mParameters(cameraName, imageWidth, imageHeight, 282 | k2, k3, k4, k5, mu, mv, u0, v0) 283 | { 284 | // Inverse camera projection matrix parameters 285 | m_inv_K11 = 1.0 / mParameters.mu(); 286 | m_inv_K13 = -mParameters.u0() / mParameters.mu(); 287 | m_inv_K22 = 1.0 / mParameters.mv(); 288 | m_inv_K23 = -mParameters.v0() / mParameters.mv(); 289 | } 290 | 291 | EquidistantCamera::EquidistantCamera(const EquidistantCamera::Parameters& params) 292 | : mParameters(params) 293 | { 294 | // Inverse camera projection matrix parameters 295 | m_inv_K11 = 1.0 / mParameters.mu(); 296 | m_inv_K13 = -mParameters.u0() / mParameters.mu(); 297 | m_inv_K22 = 1.0 / mParameters.mv(); 298 | m_inv_K23 = -mParameters.v0() / mParameters.mv(); 299 | } 300 | 301 | Camera::ModelType 302 | EquidistantCamera::modelType(void) const 303 | { 304 | return mParameters.modelType(); 305 | } 306 | 307 | const std::string& 308 | EquidistantCamera::cameraName(void) const 309 | { 310 | return mParameters.cameraName(); 311 | } 312 | 313 | int 314 | EquidistantCamera::imageWidth(void) const 315 | { 316 | return mParameters.imageWidth(); 317 | } 318 | 319 | int 320 | EquidistantCamera::imageHeight(void) const 321 | { 322 | return mParameters.imageHeight(); 323 | } 324 | 325 | /** 326 | * \brief Lifts a point from the image plane to the unit sphere 327 | * 328 | * \param p image coordinates 329 | * \param P coordinates of the point on the sphere 330 | */ 331 | void 332 | EquidistantCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const 333 | { 334 | liftProjective(p, P); 335 | } 336 | 337 | /** 338 | * \brief Lifts a point from the image plane to its projective ray 339 | * 340 | * \param p image coordinates 341 | * \param P coordinates of the projective ray 342 | */ 343 | void 344 | EquidistantCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const 345 | { 346 | // Lift points to normalised plane 347 | Eigen::Vector2d p_u; 348 | p_u << m_inv_K11 * p(0) + m_inv_K13, 349 | m_inv_K22 * p(1) + m_inv_K23; 350 | 351 | // Obtain a projective ray 352 | double theta, phi; 353 | backprojectSymmetric(p_u, theta, phi); 354 | 355 | P(0) = sin(theta) * cos(phi); 356 | P(1) = sin(theta) * sin(phi); 357 | P(2) = cos(theta); 358 | } 359 | 360 | /** 361 | * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) 362 | * 363 | * \param P 3D point coordinates 364 | * \param p return value, contains the image point coordinates 365 | */ 366 | void 367 | EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const 368 | { 369 | double theta = acos(P(2) / P.norm()); 370 | double phi = atan2(P(1), P(0)); 371 | 372 | Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); 373 | 374 | // Apply generalised projection matrix 375 | p << mParameters.mu() * p_u(0) + mParameters.u0(), 376 | mParameters.mv() * p_u(1) + mParameters.v0(); 377 | } 378 | 379 | 380 | /** 381 | * \brief Project a 3D point to the image plane and calculate Jacobian 382 | * 383 | * \param P 3D point coordinates 384 | * \param p return value, contains the image point coordinates 385 | */ 386 | void 387 | EquidistantCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 388 | Eigen::Matrix& J) const 389 | { 390 | double theta = acos(P(2) / P.norm()); 391 | double phi = atan2(P(1), P(0)); 392 | 393 | Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), mParameters.k5(), theta) * Eigen::Vector2d(cos(phi), sin(phi)); 394 | 395 | // Apply generalised projection matrix 396 | p << mParameters.mu() * p_u(0) + mParameters.u0(), 397 | mParameters.mv() * p_u(1) + mParameters.v0(); 398 | } 399 | 400 | /** 401 | * \brief Projects an undistorted 2D point p_u to the image plane 402 | * 403 | * \param p_u 2D point coordinates 404 | * \return image point coordinates 405 | */ 406 | void 407 | EquidistantCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const 408 | { 409 | // Eigen::Vector2d p_d; 410 | // 411 | // if (m_noDistortion) 412 | // { 413 | // p_d = p_u; 414 | // } 415 | // else 416 | // { 417 | // // Apply distortion 418 | // Eigen::Vector2d d_u; 419 | // distortion(p_u, d_u); 420 | // p_d = p_u + d_u; 421 | // } 422 | // 423 | // // Apply generalised projection matrix 424 | // p << mParameters.gamma1() * p_d(0) + mParameters.u0(), 425 | // mParameters.gamma2() * p_d(1) + mParameters.v0(); 426 | } 427 | 428 | void 429 | EquidistantCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const 430 | { 431 | cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); 432 | 433 | cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); 434 | cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); 435 | 436 | for (int v = 0; v < imageSize.height; ++v) 437 | { 438 | for (int u = 0; u < imageSize.width; ++u) 439 | { 440 | double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; 441 | double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; 442 | 443 | double theta, phi; 444 | backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); 445 | 446 | Eigen::Vector3d P; 447 | P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); 448 | 449 | Eigen::Vector2d p; 450 | spaceToPlane(P, p); 451 | 452 | mapX.at(v,u) = p(0); 453 | mapY.at(v,u) = p(1); 454 | } 455 | } 456 | 457 | cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); 458 | } 459 | 460 | cv::Mat 461 | EquidistantCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 462 | float fx, float fy, 463 | cv::Size imageSize, 464 | float cx, float cy, 465 | cv::Mat rmat) const 466 | { 467 | if (imageSize == cv::Size(0, 0)) 468 | { 469 | imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); 470 | } 471 | 472 | cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); 473 | cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); 474 | 475 | Eigen::Matrix3f K_rect; 476 | 477 | if (cx == -1.0f && cy == -1.0f) 478 | { 479 | K_rect << fx, 0, imageSize.width / 2, 480 | 0, fy, imageSize.height / 2, 481 | 0, 0, 1; 482 | } 483 | else 484 | { 485 | K_rect << fx, 0, cx, 486 | 0, fy, cy, 487 | 0, 0, 1; 488 | } 489 | 490 | if (fx == -1.0f || fy == -1.0f) 491 | { 492 | K_rect(0,0) = mParameters.mu(); 493 | K_rect(1,1) = mParameters.mv(); 494 | } 495 | 496 | Eigen::Matrix3f K_rect_inv = K_rect.inverse(); 497 | 498 | Eigen::Matrix3f R, R_inv; 499 | cv::cv2eigen(rmat, R); 500 | R_inv = R.inverse(); 501 | 502 | for (int v = 0; v < imageSize.height; ++v) 503 | { 504 | for (int u = 0; u < imageSize.width; ++u) 505 | { 506 | Eigen::Vector3f xo; 507 | xo << u, v, 1; 508 | 509 | Eigen::Vector3f uo = R_inv * K_rect_inv * xo; 510 | 511 | Eigen::Vector2d p; 512 | spaceToPlane(uo.cast(), p); 513 | 514 | mapX.at(v,u) = p(0); 515 | mapY.at(v,u) = p(1); 516 | } 517 | } 518 | 519 | cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); 520 | 521 | cv::Mat K_rect_cv; 522 | cv::eigen2cv(K_rect, K_rect_cv); 523 | return K_rect_cv; 524 | } 525 | 526 | int 527 | EquidistantCamera::parameterCount(void) const 528 | { 529 | return 8; 530 | } 531 | 532 | const EquidistantCamera::Parameters& 533 | EquidistantCamera::getParameters(void) const 534 | { 535 | return mParameters; 536 | } 537 | 538 | void 539 | EquidistantCamera::setParameters(const EquidistantCamera::Parameters& parameters) 540 | { 541 | mParameters = parameters; 542 | 543 | // Inverse camera projection matrix parameters 544 | m_inv_K11 = 1.0 / mParameters.mu(); 545 | m_inv_K13 = -mParameters.u0() / mParameters.mu(); 546 | m_inv_K22 = 1.0 / mParameters.mv(); 547 | m_inv_K23 = -mParameters.v0() / mParameters.mv(); 548 | } 549 | 550 | void 551 | EquidistantCamera::readParameters(const std::vector& parameterVec) 552 | { 553 | if (parameterVec.size() != parameterCount()) 554 | { 555 | return; 556 | } 557 | 558 | Parameters params = getParameters(); 559 | 560 | params.k2() = parameterVec.at(0); 561 | params.k3() = parameterVec.at(1); 562 | params.k4() = parameterVec.at(2); 563 | params.k5() = parameterVec.at(3); 564 | params.mu() = parameterVec.at(4); 565 | params.mv() = parameterVec.at(5); 566 | params.u0() = parameterVec.at(6); 567 | params.v0() = parameterVec.at(7); 568 | 569 | setParameters(params); 570 | } 571 | 572 | void 573 | EquidistantCamera::writeParameters(std::vector& parameterVec) const 574 | { 575 | parameterVec.resize(parameterCount()); 576 | parameterVec.at(0) = mParameters.k2(); 577 | parameterVec.at(1) = mParameters.k3(); 578 | parameterVec.at(2) = mParameters.k4(); 579 | parameterVec.at(3) = mParameters.k5(); 580 | parameterVec.at(4) = mParameters.mu(); 581 | parameterVec.at(5) = mParameters.mv(); 582 | parameterVec.at(6) = mParameters.u0(); 583 | parameterVec.at(7) = mParameters.v0(); 584 | } 585 | 586 | void 587 | EquidistantCamera::writeParametersToYamlFile(const std::string& filename) const 588 | { 589 | mParameters.writeToYamlFile(filename); 590 | } 591 | 592 | std::string 593 | EquidistantCamera::parametersToString(void) const 594 | { 595 | std::ostringstream oss; 596 | oss << mParameters; 597 | 598 | return oss.str(); 599 | } 600 | 601 | void 602 | EquidistantCamera::fitOddPoly(const std::vector& x, const std::vector& y, 603 | int n, std::vector& coeffs) const 604 | { 605 | std::vector pows; 606 | for (int i = 1; i <= n; i += 2) 607 | { 608 | pows.push_back(i); 609 | } 610 | 611 | Eigen::MatrixXd X(x.size(), pows.size()); 612 | Eigen::MatrixXd Y(y.size(), 1); 613 | for (size_t i = 0; i < x.size(); ++i) 614 | { 615 | for (size_t j = 0; j < pows.size(); ++j) 616 | { 617 | X(i,j) = pow(x.at(i), pows.at(j)); 618 | } 619 | Y(i,0) = y.at(i); 620 | } 621 | 622 | Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y; 623 | 624 | coeffs.resize(A.rows()); 625 | for (int i = 0; i < A.rows(); ++i) 626 | { 627 | coeffs.at(i) = A(i,0); 628 | } 629 | } 630 | 631 | void 632 | EquidistantCamera::backprojectSymmetric(const Eigen::Vector2d& p_u, 633 | double& theta, double& phi) const 634 | { 635 | double tol = 1e-10; 636 | double p_u_norm = p_u.norm(); 637 | 638 | if (p_u_norm < 1e-10) 639 | { 640 | phi = 0.0; 641 | } 642 | else 643 | { 644 | phi = atan2(p_u(1), p_u(0)); 645 | } 646 | 647 | int npow = 9; 648 | if (mParameters.k5() == 0.0) 649 | { 650 | npow -= 2; 651 | } 652 | if (mParameters.k4() == 0.0) 653 | { 654 | npow -= 2; 655 | } 656 | if (mParameters.k3() == 0.0) 657 | { 658 | npow -= 2; 659 | } 660 | if (mParameters.k2() == 0.0) 661 | { 662 | npow -= 2; 663 | } 664 | 665 | Eigen::MatrixXd coeffs(npow + 1, 1); 666 | coeffs.setZero(); 667 | coeffs(0) = -p_u_norm; 668 | coeffs(1) = 1.0; 669 | 670 | if (npow >= 3) 671 | { 672 | coeffs(3) = mParameters.k2(); 673 | } 674 | if (npow >= 5) 675 | { 676 | coeffs(5) = mParameters.k3(); 677 | } 678 | if (npow >= 7) 679 | { 680 | coeffs(7) = mParameters.k4(); 681 | } 682 | if (npow >= 9) 683 | { 684 | coeffs(9) = mParameters.k5(); 685 | } 686 | 687 | if (npow == 1) 688 | { 689 | theta = p_u_norm; 690 | } 691 | else 692 | { 693 | // Get eigenvalues of companion matrix corresponding to polynomial. 694 | // Eigenvalues correspond to roots of polynomial. 695 | Eigen::MatrixXd A(npow, npow); 696 | A.setZero(); 697 | A.block(1, 0, npow - 1, npow - 1).setIdentity(); 698 | A.col(npow - 1) = - coeffs.block(0, 0, npow, 1) / coeffs(npow); 699 | 700 | Eigen::EigenSolver es(A); 701 | Eigen::MatrixXcd eigval = es.eigenvalues(); 702 | 703 | std::vector thetas; 704 | for (int i = 0; i < eigval.rows(); ++i) 705 | { 706 | if (fabs(eigval(i).imag()) > tol) 707 | { 708 | continue; 709 | } 710 | 711 | double t = eigval(i).real(); 712 | 713 | if (t < -tol) 714 | { 715 | continue; 716 | } 717 | else if (t < 0.0) 718 | { 719 | t = 0.0; 720 | } 721 | 722 | thetas.push_back(t); 723 | } 724 | 725 | if (thetas.empty()) 726 | { 727 | theta = p_u_norm; 728 | } 729 | else 730 | { 731 | theta = *std::min_element(thetas.begin(), thetas.end()); 732 | } 733 | } 734 | } 735 | 736 | } 737 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/MapFinder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include "defraction_map_finder/MapFinder.hpp" 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "defraction_map_finder/CameraFactory.h" 35 | 36 | 37 | using namespace std; 38 | using namespace cv; 39 | using namespace Eigen; 40 | using namespace camodocal; 41 | 42 | MapFinder::MapFinder(int W, int H, float d_0, float d0off, float d_1, float n_g, float n_w, 43 | const sensor_msgs::CameraInfo info,CameraPtr realCam){ 44 | ImgWidth=W; 45 | ImgHeight=H; 46 | d0=d_0; 47 | d0Virt_offset=d0off; 48 | d1=d_1; 49 | ng=n_g; 50 | nw=n_w; 51 | 52 | Mat mx (ImgHeight, ImgWidth, DataType::type); 53 | Mat my (ImgHeight, ImgWidth, DataType::type); 54 | mapx=mx; 55 | mapy=my; 56 | 57 | VirtualCamera.fromCameraInfo(info); 58 | RealCamera.fromCameraInfo(info); 59 | 60 | jir_refractive_image_geometry_msgs::PlanarRefractionInfo ref; 61 | ref.header = info.header; 62 | ref.normal[0]= 0; 63 | ref.normal[1]= 0; 64 | ref.normal[2]= -1; 65 | ref.d_0 = d0; 66 | ref.d_1 = d1; 67 | ref.n_glass = ng; 68 | ref.n_water = nw; 69 | 70 | RefractionInfo=ref; 71 | 72 | RealCamera.fromPlanarRefractionInfo(ref); 73 | camera_info=info; 74 | myCamera=realCam; 75 | 76 | 77 | } 78 | 79 | 80 | 81 | void MapFinder::init_setup (int W, int H, float d_0, float d0off, float d_1, float n_g, float n_w, 82 | const sensor_msgs::CameraInfo info,CameraPtr realCam){ 83 | ImgWidth=W; 84 | ImgHeight=H; 85 | d0=d_0; 86 | d0Virt_offset=d0off; 87 | d1=d_1; 88 | ng=n_g; 89 | nw=n_w; 90 | 91 | 92 | Mat mx (ImgHeight, ImgWidth, DataType::type); 93 | Mat my (ImgHeight, ImgWidth, DataType::type); 94 | mapx=mx; 95 | mapy=my; 96 | 97 | VirtualCamera.fromCameraInfo(info); 98 | RealCamera.fromCameraInfo(info); 99 | 100 | jir_refractive_image_geometry_msgs::PlanarRefractionInfo ref; 101 | ref.header = info.header; 102 | ref.normal[0]= 0; 103 | ref.normal[1]= 0; 104 | ref.normal[2]= -1; 105 | ref.d_0 = d0; 106 | ref.d_1 = d1; 107 | ref.n_glass = ng; 108 | ref.n_water = nw; 109 | 110 | RefractionInfo=ref; 111 | 112 | RealCamera.fromPlanarRefractionInfo(ref); 113 | camera_info=info; 114 | myCamera=realCam; 115 | 116 | } 117 | 118 | 119 | void MapFinder::generate3Dpoints(){ 120 | Vector3d plane_normal; 121 | plane_normal[0]=0.0; 122 | plane_normal[1]=0.0; 123 | plane_normal[2]=-1.0; 124 | 125 | points3D.resize( ImgHeight*ImgWidth ); 126 | 127 | #pragma omp parallel for 128 | for (int i=0; i ray = VirtualCamera.projectPixelTo3dRay(j,i); 131 | ray.offset[2]+=d0Virt_offset; 132 | 133 | Vector3d point; 134 | intersect(ray, plane_normal, 5.0, point); 135 | 136 | points3D[ i*ImgWidth + j ] = point; 137 | } 138 | } 139 | } 140 | 141 | void MapFinder::projectPoints(){ 142 | Vector3d rotatedNormal (0,0,-1); 143 | 144 | rotatedNormal.normalize(); 145 | RefractionInfo.normal[0]=rotatedNormal[0]; 146 | RefractionInfo.normal[1]=rotatedNormal[1]; 147 | RefractionInfo.normal[2]=rotatedNormal[2]; 148 | 149 | #pragma omp parallel for 150 | for (int i=0; i(i,j)=temp.x; 164 | mapy.at(i,j)=temp.y; 165 | } 166 | } 167 | } 168 | 169 | cv::Mat MapFinder::getMask() const { 170 | Mat mask (myCamera->imageHeight(), myCamera->imageWidth(), CV_8UC1, Scalar::all(255)); 171 | 172 | Mat mask_out; 173 | 174 | remap( mask, mask_out, mapx, mapy, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar::all(0) ); 175 | 176 | Mat mask_thresh; 177 | threshold(mask_out, mask_thresh, 250, 255, THRESH_BINARY); 178 | 179 | return mask_thresh; 180 | } 181 | 182 | void MapFinder::saveMaps (const std::string& filename){ 183 | cv::Mat mask = getMask(); 184 | 185 | FileStorage fs(filename, FileStorage::WRITE); 186 | 187 | 188 | Mat K (3, 3, DataType::type); 189 | for (int i=0; i<3; i++){ 190 | for (int j=0;j<3;j++){ 191 | K.at(i,j)=camera_info.K[i*3+j]; 192 | } 193 | } 194 | 195 | fs << "camera_info" << K; 196 | 197 | fs << "input_rows" << myCamera->imageHeight(); 198 | fs << "input_cols" << myCamera->imageWidth(); 199 | fs << "mapx" << mapx; 200 | fs << "mapy" << mapy; 201 | fs << "mask" << mask; 202 | 203 | 204 | } 205 | 206 | 207 | 208 | Point2d MapFinder::projectPoint (Vector3d point3D, bool notRefracted){ 209 | 210 | Vector2d pointDistorted; 211 | Vector3d xyz; 212 | 213 | if(notRefracted){ 214 | xyz[0]=0; 215 | xyz[1]=0; 216 | xyz[2]=RefractionInfo.d_1; 217 | } 218 | else { 219 | point_on_inside_of_refractive_plane_analytic (point3D, RefractionInfo, xyz); 220 | } 221 | 222 | 223 | 224 | myCamera->spaceToPlane(xyz, pointDistorted); 225 | 226 | Point2d tmp; 227 | tmp.x=pointDistorted[0]; 228 | tmp.y=pointDistorted[1]; 229 | 230 | return tmp; 231 | } 232 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/PinholeCamera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | CamOdoCal: Automatic Intrinsic and Extrinsic Calibration of a Rig with 3 | Multiple Generic Cameras and Odometry 4 | Copyright (c) 2013 Lionel Heng 5 | 6 | This software is licensed under CC-BY-SA. For more information, visit 7 | http://http://creativecommons.org/licenses/by-sa/2.0/ 8 | 9 | If you use this software for an academic publication, please cite this paper: 10 | Lionel Heng, Bo Li, and Marc Pollefeys, CamOdoCal: Automatic Intrinsic and 11 | Extrinsic Calibration of a Rig with Multiple Generic Cameras and Odometry, 12 | In Proc. IEEE/RSJ International Conference on Intelligent Robots 13 | and Systems (IROS), 2013. 14 | */ 15 | 16 | #include "defraction_map_finder/PinholeCamera.h" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | //include "../gpl/gpl.h" 27 | 28 | namespace camodocal 29 | { 30 | 31 | PinholeCamera::Parameters::Parameters() 32 | : Camera::Parameters(PINHOLE) 33 | , m_k1(0.0) 34 | , m_k2(0.0) 35 | , m_p1(0.0) 36 | , m_p2(0.0) 37 | , m_fx(0.0) 38 | , m_fy(0.0) 39 | , m_cx(0.0) 40 | , m_cy(0.0) 41 | { 42 | 43 | } 44 | 45 | PinholeCamera::Parameters::Parameters(const std::string& cameraName, 46 | int w, int h, 47 | double k1, double k2, 48 | double p1, double p2, 49 | double fx, double fy, 50 | double cx, double cy) 51 | : Camera::Parameters(PINHOLE, cameraName, w, h) 52 | , m_k1(k1) 53 | , m_k2(k2) 54 | , m_p1(p1) 55 | , m_p2(p2) 56 | , m_fx(fx) 57 | , m_fy(fy) 58 | , m_cx(cx) 59 | , m_cy(cy) 60 | { 61 | } 62 | 63 | double& 64 | PinholeCamera::Parameters::k1(void) 65 | { 66 | return m_k1; 67 | } 68 | 69 | double& 70 | PinholeCamera::Parameters::k2(void) 71 | { 72 | return m_k2; 73 | } 74 | 75 | double& 76 | PinholeCamera::Parameters::p1(void) 77 | { 78 | return m_p1; 79 | } 80 | 81 | double& 82 | PinholeCamera::Parameters::p2(void) 83 | { 84 | return m_p2; 85 | } 86 | 87 | double& 88 | PinholeCamera::Parameters::fx(void) 89 | { 90 | return m_fx; 91 | } 92 | 93 | double& 94 | PinholeCamera::Parameters::fy(void) 95 | { 96 | return m_fy; 97 | } 98 | 99 | double& 100 | PinholeCamera::Parameters::cx(void) 101 | { 102 | return m_cx; 103 | } 104 | 105 | double& 106 | PinholeCamera::Parameters::cy(void) 107 | { 108 | return m_cy; 109 | } 110 | 111 | double 112 | PinholeCamera::Parameters::k1(void) const 113 | { 114 | return m_k1; 115 | } 116 | 117 | double 118 | PinholeCamera::Parameters::k2(void) const 119 | { 120 | return m_k2; 121 | } 122 | 123 | double 124 | PinholeCamera::Parameters::p1(void) const 125 | { 126 | return m_p1; 127 | } 128 | 129 | double 130 | PinholeCamera::Parameters::p2(void) const 131 | { 132 | return m_p2; 133 | } 134 | 135 | double 136 | PinholeCamera::Parameters::fx(void) const 137 | { 138 | return m_fx; 139 | } 140 | 141 | double 142 | PinholeCamera::Parameters::fy(void) const 143 | { 144 | return m_fy; 145 | } 146 | 147 | double 148 | PinholeCamera::Parameters::cx(void) const 149 | { 150 | return m_cx; 151 | } 152 | 153 | double 154 | PinholeCamera::Parameters::cy(void) const 155 | { 156 | return m_cy; 157 | } 158 | 159 | bool 160 | PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) 161 | { 162 | cv::FileStorage fs(filename, cv::FileStorage::READ); 163 | 164 | if (!fs.isOpened()) 165 | { 166 | return false; 167 | } 168 | 169 | if (!fs["model_type"].isNone()) 170 | { 171 | std::string sModelType; 172 | fs["model_type"] >> sModelType; 173 | 174 | if (sModelType.compare("PINHOLE") != 0) 175 | { 176 | return false; 177 | } 178 | } 179 | 180 | m_modelType = PINHOLE; 181 | fs["camera_name"] >> m_cameraName; 182 | m_imageWidth = static_cast(fs["image_width"]); 183 | m_imageHeight = static_cast(fs["image_height"]); 184 | 185 | cv::FileNode n = fs["distortion_parameters"]; 186 | m_k1 = static_cast(n["k1"]); 187 | m_k2 = static_cast(n["k2"]); 188 | m_p1 = static_cast(n["p1"]); 189 | m_p2 = static_cast(n["p2"]); 190 | 191 | n = fs["projection_parameters"]; 192 | m_fx = static_cast(n["fx"]); 193 | m_fy = static_cast(n["fy"]); 194 | m_cx = static_cast(n["cx"]); 195 | m_cy = static_cast(n["cy"]); 196 | 197 | return true; 198 | } 199 | 200 | void 201 | PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const 202 | { 203 | cv::FileStorage fs(filename, cv::FileStorage::WRITE); 204 | 205 | fs << "model_type" << "PINHOLE"; 206 | fs << "camera_name" << m_cameraName; 207 | fs << "image_width" << m_imageWidth; 208 | fs << "image_height" << m_imageHeight; 209 | 210 | // radial distortion: k1, k2 211 | // tangential distortion: p1, p2 212 | fs << "distortion_parameters"; 213 | fs << "{" << "k1" << m_k1 214 | << "k2" << m_k2 215 | << "p1" << m_p1 216 | << "p2" << m_p2 << "}"; 217 | 218 | // projection: fx, fy, cx, cy 219 | fs << "projection_parameters"; 220 | fs << "{" << "fx" << m_fx 221 | << "fy" << m_fy 222 | << "cx" << m_cx 223 | << "cy" << m_cy << "}"; 224 | 225 | fs.release(); 226 | } 227 | 228 | PinholeCamera::Parameters& 229 | PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other) 230 | { 231 | if (this != &other) 232 | { 233 | m_modelType = other.m_modelType; 234 | m_cameraName = other.m_cameraName; 235 | m_imageWidth = other.m_imageWidth; 236 | m_imageHeight = other.m_imageHeight; 237 | m_k1 = other.m_k1; 238 | m_k2 = other.m_k2; 239 | m_p1 = other.m_p1; 240 | m_p2 = other.m_p2; 241 | m_fx = other.m_fx; 242 | m_fy = other.m_fy; 243 | m_cx = other.m_cx; 244 | m_cy = other.m_cy; 245 | } 246 | 247 | return *this; 248 | } 249 | 250 | std::ostream& 251 | operator<< (std::ostream& out, const PinholeCamera::Parameters& params) 252 | { 253 | out << "Camera Parameters:" << std::endl; 254 | out << " model_type " << "PINHOLE" << std::endl; 255 | out << " camera_name " << params.m_cameraName << std::endl; 256 | out << " image_width " << params.m_imageWidth << std::endl; 257 | out << " image_height " << params.m_imageHeight << std::endl; 258 | 259 | // radial distortion: k1, k2 260 | // tangential distortion: p1, p2 261 | out << "Distortion Parameters" << std::endl; 262 | out << " k1 " << params.m_k1 << std::endl 263 | << " k2 " << params.m_k2 << std::endl 264 | << " p1 " << params.m_p1 << std::endl 265 | << " p2 " << params.m_p2 << std::endl; 266 | 267 | // projection: fx, fy, cx, cy 268 | out << "Projection Parameters" << std::endl; 269 | out << " fx " << params.m_fx << std::endl 270 | << " fy " << params.m_fy << std::endl 271 | << " cx " << params.m_cx << std::endl 272 | << " cy " << params.m_cy << std::endl; 273 | 274 | return out; 275 | } 276 | 277 | PinholeCamera::PinholeCamera() 278 | : m_inv_K11(1.0) 279 | , m_inv_K13(0.0) 280 | , m_inv_K22(1.0) 281 | , m_inv_K23(0.0) 282 | , m_noDistortion(true) 283 | { 284 | 285 | } 286 | 287 | PinholeCamera::PinholeCamera(const std::string& cameraName, 288 | int imageWidth, int imageHeight, 289 | double k1, double k2, double p1, double p2, 290 | double fx, double fy, double cx, double cy) 291 | : mParameters(cameraName, imageWidth, imageHeight, 292 | k1, k2, p1, p2, fx, fy, cx, cy) 293 | { 294 | if ((mParameters.k1() == 0.0) && 295 | (mParameters.k2() == 0.0) && 296 | (mParameters.p1() == 0.0) && 297 | (mParameters.p2() == 0.0)) 298 | { 299 | m_noDistortion = true; 300 | } 301 | else 302 | { 303 | m_noDistortion = false; 304 | } 305 | 306 | // Inverse camera projection matrix parameters 307 | m_inv_K11 = 1.0 / mParameters.fx(); 308 | m_inv_K13 = -mParameters.cx() / mParameters.fx(); 309 | m_inv_K22 = 1.0 / mParameters.fy(); 310 | m_inv_K23 = -mParameters.cy() / mParameters.fy(); 311 | } 312 | 313 | PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) 314 | : mParameters(params) 315 | { 316 | if ((mParameters.k1() == 0.0) && 317 | (mParameters.k2() == 0.0) && 318 | (mParameters.p1() == 0.0) && 319 | (mParameters.p2() == 0.0)) 320 | { 321 | m_noDistortion = true; 322 | } 323 | else 324 | { 325 | m_noDistortion = false; 326 | } 327 | 328 | // Inverse camera projection matrix parameters 329 | m_inv_K11 = 1.0 / mParameters.fx(); 330 | m_inv_K13 = -mParameters.cx() / mParameters.fx(); 331 | m_inv_K22 = 1.0 / mParameters.fy(); 332 | m_inv_K23 = -mParameters.cy() / mParameters.fy(); 333 | } 334 | 335 | Camera::ModelType 336 | PinholeCamera::modelType(void) const 337 | { 338 | return mParameters.modelType(); 339 | } 340 | 341 | const std::string& 342 | PinholeCamera::cameraName(void) const 343 | { 344 | return mParameters.cameraName(); 345 | } 346 | 347 | int 348 | PinholeCamera::imageWidth(void) const 349 | { 350 | return mParameters.imageWidth(); 351 | } 352 | 353 | int 354 | PinholeCamera::imageHeight(void) const 355 | { 356 | return mParameters.imageHeight(); 357 | } 358 | /* 359 | void 360 | PinholeCamera::estimateIntrinsics(const cv::Size& boardSize, 361 | const std::vector< std::vector >& objectPoints, 362 | const std::vector< std::vector >& imagePoints) 363 | { 364 | // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000 365 | 366 | Parameters params = getParameters(); 367 | 368 | params.k1() = 0.0; 369 | params.k2() = 0.0; 370 | params.p1() = 0.0; 371 | params.p2() = 0.0; 372 | 373 | double cx = params.imageWidth() / 2.0; 374 | double cy = params.imageHeight() / 2.0; 375 | params.cx() = cx; 376 | params.cy() = cy; 377 | 378 | size_t nImages = imagePoints.size(); 379 | 380 | cv::Mat A(nImages * 2, 2, CV_64F); 381 | cv::Mat b(nImages * 2, 1, CV_64F); 382 | 383 | for (size_t i = 0; i < nImages; ++i) 384 | { 385 | const std::vector& oPoints = objectPoints.at(i); 386 | 387 | std::vector M(oPoints.size()); 388 | for (size_t j = 0; j < M.size(); ++j) 389 | { 390 | M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y); 391 | } 392 | 393 | cv::Mat H = cv::findHomography(M, imagePoints.at(i)); 394 | 395 | H.at(0,0) -= H.at(2,0) * cx; 396 | H.at(0,1) -= H.at(2,1) * cx; 397 | H.at(0,2) -= H.at(2,2) * cx; 398 | H.at(1,0) -= H.at(2,0) * cy; 399 | H.at(1,1) -= H.at(2,1) * cy; 400 | H.at(1,2) -= H.at(2,2) * cy; 401 | 402 | double h[3], v[3], d1[3], d2[3]; 403 | double n[4] = {0,0,0,0}; 404 | 405 | for (int j = 0; j < 3; ++j) 406 | { 407 | double t0 = H.at(j,0); 408 | double t1 = H.at(j,1); 409 | h[j] = t0; v[j] = t1; 410 | d1[j] = (t0 + t1) * 0.5; 411 | d2[j] = (t0 - t1) * 0.5; 412 | n[0] += t0 * t0; n[1] += t1 * t1; 413 | n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j]; 414 | } 415 | 416 | for (int j = 0; j < 4; ++j) 417 | { 418 | n[j] = 1.0 / sqrt(n[j]); 419 | } 420 | 421 | for (int j = 0; j < 3; ++j) 422 | { 423 | h[j] *= n[0]; v[j] *= n[1]; 424 | d1[j] *= n[2]; d2[j] *= n[3]; 425 | } 426 | 427 | A.at(i * 2, 0) = h[0] * v[0]; 428 | A.at(i * 2, 1) = h[1] * v[1]; 429 | A.at(i * 2 + 1, 0) = d1[0] * d2[0]; 430 | A.at(i * 2 + 1, 1) = d1[1] * d2[1]; 431 | b.at(i * 2, 0) = -h[2] * v[2]; 432 | b.at(i * 2 + 1, 0) = -d1[2] * d2[2]; 433 | } 434 | 435 | cv::Mat f(2, 1, CV_64F); 436 | cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU); 437 | 438 | params.fx() = sqrt(fabs(1.0 / f.at(0))); 439 | params.fy() = sqrt(fabs(1.0 / f.at(1))); 440 | 441 | setParameters(params); 442 | } 443 | 444 | 445 | void 446 | PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const 447 | { 448 | liftProjective(p, P); 449 | 450 | P.normalize(); 451 | } 452 | 453 | 454 | void 455 | PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const 456 | { 457 | double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; 458 | double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; 459 | double lambda; 460 | 461 | // Lift points to normalised plane 462 | mx_d = m_inv_K11 * p(0) + m_inv_K13; 463 | my_d = m_inv_K22 * p(1) + m_inv_K23; 464 | 465 | if (m_noDistortion) 466 | { 467 | mx_u = mx_d; 468 | my_u = my_d; 469 | } 470 | else 471 | { 472 | if (0) 473 | { 474 | double k1 = mParameters.k1(); 475 | double k2 = mParameters.k2(); 476 | double p1 = mParameters.p1(); 477 | double p2 = mParameters.p2(); 478 | 479 | // Apply inverse distortion model 480 | // proposed by Heikkila 481 | mx2_d = mx_d*mx_d; 482 | my2_d = my_d*my_d; 483 | mxy_d = mx_d*my_d; 484 | rho2_d = mx2_d+my2_d; 485 | rho4_d = rho2_d*rho2_d; 486 | radDist_d = k1*rho2_d+k2*rho4_d; 487 | Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; 488 | Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; 489 | inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); 490 | 491 | mx_u = mx_d - inv_denom_d*Dx_d; 492 | my_u = my_d - inv_denom_d*Dy_d; 493 | } 494 | else 495 | { 496 | // Recursive distortion model 497 | int n = 8; 498 | Eigen::Vector2d d_u; 499 | distortion(Eigen::Vector2d(mx_d, my_d), d_u); 500 | // Approximate value 501 | mx_u = mx_d - d_u(0); 502 | my_u = my_d - d_u(1); 503 | 504 | for (int i = 1; i < n; ++i) 505 | { 506 | distortion(Eigen::Vector2d(mx_u, my_u), d_u); 507 | mx_u = mx_d - d_u(0); 508 | my_u = my_d - d_u(1); 509 | } 510 | } 511 | } 512 | 513 | // Obtain a projective ray 514 | P << mx_u, my_u, 1.0; 515 | } 516 | */ 517 | 518 | /** 519 | * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) 520 | * 521 | * \param P 3D point coordinates 522 | * \param p return value, contains the image point coordinates 523 | */ 524 | void 525 | PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const 526 | { 527 | Eigen::Vector2d p_u, p_d; 528 | 529 | // Project points to the normalised plane 530 | p_u << P(0) / P(2), P(1) / P(2); 531 | 532 | if (m_noDistortion) 533 | { 534 | p_d = p_u; 535 | } 536 | else 537 | { 538 | // Apply distortion 539 | Eigen::Vector2d d_u; 540 | distortion(p_u, d_u); 541 | p_d = p_u + d_u; 542 | } 543 | 544 | // Apply generalised projection matrix 545 | p << mParameters.fx() * p_d(0) + mParameters.cx(), 546 | mParameters.fy() * p_d(1) + mParameters.cy(); 547 | } 548 | 549 | 550 | /** 551 | * \brief Project a 3D point to the image plane and calculate Jacobian 552 | * 553 | * \param P 3D point coordinates 554 | * \param p return value, contains the image point coordinates 555 | */ 556 | /*void 557 | PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 558 | Eigen::Matrix& J) const 559 | { 560 | Eigen::Vector2d p_u, p_d; 561 | double norm, inv_denom; 562 | double dxdmx, dydmx, dxdmy, dydmy; 563 | 564 | norm = P.norm(); 565 | // Project points to the normalised plane 566 | inv_denom = 1.0 / P(2); 567 | p_u << inv_denom * P(0), inv_denom * P(1); 568 | 569 | // Calculate jacobian 570 | double dudx = inv_denom; 571 | double dvdx = 0.0; 572 | double dudy = 0.0; 573 | double dvdy = inv_denom; 574 | inv_denom = - inv_denom * inv_denom; 575 | double dudz = P(0) * inv_denom; 576 | double dvdz = P(1) * inv_denom; 577 | 578 | if (m_noDistortion) 579 | { 580 | p_d = p_u; 581 | } 582 | else 583 | { 584 | // Apply distortion 585 | Eigen::Vector2d d_u; 586 | distortion(p_u, d_u); 587 | p_d = p_u + d_u; 588 | } 589 | 590 | double fx = mParameters.fx(); 591 | double fy = mParameters.fy(); 592 | 593 | // Make the product of the jacobians 594 | // and add projection matrix jacobian 595 | inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse 596 | dvdx = fy * (dudx * dydmx + dvdx * dydmy); 597 | dudx = inv_denom; 598 | 599 | inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse 600 | dvdy = fy * (dudy * dydmx + dvdy * dydmy); 601 | dudy = inv_denom; 602 | 603 | inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse 604 | dvdz = fy * (dudz * dydmx + dvdz * dydmy); 605 | dudz = inv_denom; 606 | 607 | // Apply generalised projection matrix 608 | p << fx * p_d(0) + mParameters.cx(), 609 | fy * p_d(1) + mParameters.cy(); 610 | 611 | J << dudx, dudy, dudz, 612 | dvdx, dvdy, dvdz; 613 | } 614 | 615 | /** 616 | * \brief Projects an undistorted 2D point p_u to the image plane 617 | * 618 | * \param p_u 2D point coordinates 619 | * \return image point coordinates 620 | */ 621 | /*void 622 | PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const 623 | { 624 | Eigen::Vector2d p_d; 625 | 626 | if (m_noDistortion) 627 | { 628 | p_d = p_u; 629 | } 630 | else 631 | { 632 | // Apply distortion 633 | Eigen::Vector2d d_u; 634 | distortion(p_u, d_u); 635 | p_d = p_u + d_u; 636 | } 637 | 638 | // Apply generalised projection matrix 639 | p << mParameters.fx() * p_d(0) + mParameters.cx(), 640 | mParameters.fy() * p_d(1) + mParameters.cy(); 641 | } 642 | 643 | /** 644 | * \brief Apply distortion to input point (from the normalised plane) 645 | * 646 | * \param p_u undistorted coordinates of point on the normalised plane 647 | * \return to obtain the distorted point: p_d = p_u + d_u 648 | */ 649 | void 650 | PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const 651 | { 652 | double k1 = mParameters.k1(); 653 | double k2 = mParameters.k2(); 654 | double p1 = mParameters.p1(); 655 | double p2 = mParameters.p2(); 656 | 657 | double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; 658 | 659 | mx2_u = p_u(0) * p_u(0); 660 | my2_u = p_u(1) * p_u(1); 661 | mxy_u = p_u(0) * p_u(1); 662 | rho2_u = mx2_u + my2_u; 663 | rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; 664 | d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), 665 | p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); 666 | } 667 | 668 | /** 669 | * \brief Apply distortion to input point (from the normalised plane) 670 | * and calculate Jacobian 671 | * 672 | * \param p_u undistorted coordinates of point on the normalised plane 673 | * \return to obtain the distorted point: p_d = p_u + d_u 674 | */ 675 | /*void 676 | PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 677 | Eigen::Matrix2d& J) const 678 | { 679 | double k1 = mParameters.k1(); 680 | double k2 = mParameters.k2(); 681 | double p1 = mParameters.p1(); 682 | double p2 = mParameters.p2(); 683 | 684 | double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; 685 | 686 | mx2_u = p_u(0) * p_u(0); 687 | my2_u = p_u(1) * p_u(1); 688 | mxy_u = p_u(0) * p_u(1); 689 | rho2_u = mx2_u + my2_u; 690 | rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; 691 | d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), 692 | p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); 693 | 694 | double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); 695 | double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); 696 | double dxdmy = dydmx; 697 | double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); 698 | 699 | J << dxdmx, dxdmy, 700 | dydmx, dydmy; 701 | } 702 | 703 | void 704 | PinholeCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const 705 | { 706 | cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); 707 | 708 | cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); 709 | cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); 710 | 711 | for (int v = 0; v < imageSize.height; ++v) 712 | { 713 | for (int u = 0; u < imageSize.width; ++u) 714 | { 715 | double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; 716 | double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; 717 | 718 | Eigen::Vector3d P; 719 | P << mx_u, my_u, 1.0; 720 | 721 | Eigen::Vector2d p; 722 | spaceToPlane(P, p); 723 | 724 | mapX.at(v,u) = p(0); 725 | mapY.at(v,u) = p(1); 726 | } 727 | } 728 | 729 | cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); 730 | } 731 | 732 | cv::Mat 733 | PinholeCamera::initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 734 | float fx, float fy, 735 | cv::Size imageSize, 736 | float cx, float cy, 737 | cv::Mat rmat) const 738 | { 739 | if (imageSize == cv::Size(0, 0)) 740 | { 741 | imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); 742 | } 743 | 744 | cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); 745 | cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); 746 | 747 | Eigen::Matrix3f R, R_inv; 748 | cv::cv2eigen(rmat, R); 749 | R_inv = R.inverse(); 750 | 751 | // assume no skew 752 | Eigen::Matrix3f K_rect; 753 | 754 | if (cx == -1.0f || cy == -1.0f) 755 | { 756 | K_rect << fx, 0, imageSize.width / 2, 757 | 0, fy, imageSize.height / 2, 758 | 0, 0, 1; 759 | } 760 | else 761 | { 762 | K_rect << fx, 0, cx, 763 | 0, fy, cy, 764 | 0, 0, 1; 765 | } 766 | 767 | if (fx == -1.0f || fy == -1.0f) 768 | { 769 | K_rect(0,0) = mParameters.fx(); 770 | K_rect(1,1) = mParameters.fy(); 771 | } 772 | 773 | Eigen::Matrix3f K_rect_inv = K_rect.inverse(); 774 | 775 | for (int v = 0; v < imageSize.height; ++v) 776 | { 777 | for (int u = 0; u < imageSize.width; ++u) 778 | { 779 | Eigen::Vector3f xo; 780 | xo << u, v, 1; 781 | 782 | Eigen::Vector3f uo = R_inv * K_rect_inv * xo; 783 | 784 | Eigen::Vector2d p; 785 | spaceToPlane(uo.cast(), p); 786 | 787 | mapX.at(v,u) = p(0); 788 | mapY.at(v,u) = p(1); 789 | } 790 | } 791 | 792 | cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); 793 | 794 | cv::Mat K_rect_cv; 795 | cv::eigen2cv(K_rect, K_rect_cv); 796 | return K_rect_cv; 797 | } 798 | 799 | int 800 | PinholeCamera::parameterCount(void) const 801 | { 802 | return 8; 803 | } 804 | */ 805 | const PinholeCamera::Parameters& 806 | PinholeCamera::getParameters(void) const 807 | { 808 | return mParameters; 809 | } 810 | 811 | void 812 | PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) 813 | { 814 | mParameters = parameters; 815 | 816 | if ((mParameters.k1() == 0.0) && 817 | (mParameters.k2() == 0.0) && 818 | (mParameters.p1() == 0.0) && 819 | (mParameters.p2() == 0.0)) 820 | { 821 | m_noDistortion = true; 822 | } 823 | else 824 | { 825 | m_noDistortion = false; 826 | } 827 | 828 | m_inv_K11 = 1.0 / mParameters.fx(); 829 | m_inv_K13 = -mParameters.cx() / mParameters.fx(); 830 | m_inv_K22 = 1.0 / mParameters.fy(); 831 | m_inv_K23 = -mParameters.cy() / mParameters.fy(); 832 | } 833 | /* 834 | void 835 | PinholeCamera::readParameters(const std::vector& parameterVec) 836 | { 837 | if (parameterVec.size() != parameterCount()) 838 | { 839 | return; 840 | } 841 | 842 | Parameters params = getParameters(); 843 | 844 | params.k1() = parameterVec.at(0); 845 | params.k2() = parameterVec.at(1); 846 | params.p1() = parameterVec.at(2); 847 | params.p2() = parameterVec.at(3); 848 | params.fx() = parameterVec.at(4); 849 | params.fy() = parameterVec.at(5); 850 | params.cx() = parameterVec.at(6); 851 | params.cy() = parameterVec.at(7); 852 | 853 | setParameters(params); 854 | } 855 | 856 | void 857 | PinholeCamera::writeParameters(std::vector& parameterVec) const 858 | { 859 | parameterVec.resize(parameterCount()); 860 | parameterVec.at(0) = mParameters.k1(); 861 | parameterVec.at(1) = mParameters.k2(); 862 | parameterVec.at(2) = mParameters.p1(); 863 | parameterVec.at(3) = mParameters.p2(); 864 | parameterVec.at(4) = mParameters.fx(); 865 | parameterVec.at(5) = mParameters.fy(); 866 | parameterVec.at(6) = mParameters.cx(); 867 | parameterVec.at(7) = mParameters.cy(); 868 | } 869 | 870 | void 871 | PinholeCamera::writeParametersToYamlFile(const std::string& filename) const 872 | { 873 | mParameters.writeToYamlFile(filename); 874 | } 875 | 876 | std::string 877 | PinholeCamera::parametersToString(void) const 878 | { 879 | std::ostringstream oss; 880 | oss << mParameters; 881 | 882 | return oss.str(); 883 | }*/ 884 | 885 | } 886 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/defraction_map_finder/src/defraction_map_finder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include "defraction_map_finder/MapFinder.hpp" 39 | #include "defraction_map_finder/CameraFactory.h" 40 | #include 41 | 42 | using namespace cv; 43 | using namespace std; 44 | using namespace Eigen; 45 | 46 | using namespace camodocal; 47 | 48 | 49 | int main(int argc, char** argv) 50 | { 51 | //input calibration file 52 | string intr="camera_left.yaml"; 53 | 54 | //output correction file 55 | string mapsResults; 56 | mapsResults="correctionMap.yaml"; 57 | 58 | //**** virtual camra params **** 59 | int W=1024; 60 | int H=768; 61 | float f=600; 62 | 63 | //setup parameters, d0 and d0off optimized in simulation, depending on d1, ng and nw 64 | float d_0=0.0014; 65 | float d0off=0.00082; 66 | float d_1=0.01; 67 | float n_g=1.492; 68 | float n_w=1.342; 69 | //* 70 | 71 | 72 | //**** create camera info **** 73 | sensor_msgs::CameraInfo info; 74 | 75 | info.height=H; 76 | info.width=W; 77 | 78 | info.K[0]=f; 79 | info.K[1]=0; 80 | info.K[2]=W/2.0; 81 | 82 | info.K[3]=0; 83 | info.K[4]=f; 84 | info.K[5]=H/2.0; 85 | 86 | info.K[6]=0; 87 | info.K[7]=0; 88 | info.K[8]=1; 89 | 90 | info.R[0] = info.R[4] = info.R[8] = 1.0; 91 | 92 | info.P[0] = f; 93 | info.P[2] = W/2.0; 94 | info.P[5] = f; 95 | info.P[6] = H/2.0; 96 | info.P[10] = 1.0; 97 | 98 | info.binning_x = 0; 99 | info.binning_y = 0; 100 | info.roi.height = H; 101 | info.roi.width = W; 102 | info.roi.do_rectify = false; 103 | //* 104 | 105 | camodocal::CameraPtr distCam = CameraFactory::instance()->generateCameraFromYamlFile(intr); 106 | 107 | MapFinder find(W, H, d_0, d0off, d_1, n_g, n_w, info, distCam); 108 | cout<<"object created\n"; 109 | find.generate3Dpoints(); 110 | cout<<"3D points generated\n"; 111 | 112 | find.projectPoints(); 113 | 114 | cout<<"Points projected\n"; 115 | 116 | find.saveMaps(mapsResults); 117 | cout<<"Results saved\n"; 118 | 119 | return 0; 120 | } 121 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_image_remapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jir_image_remapper) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | image_transport 9 | jir_rectification_remap_lib 10 | message_filters 11 | roscpp 12 | sensor_msgs 13 | cv_bridge 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 36 | ## pulled in transitively but can be declared for certainty nonetheless: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # sensor_msgs 74 | # ) 75 | 76 | ################################### 77 | ## catkin specific configuration ## 78 | ################################### 79 | ## The catkin_package macro generates cmake config files for your package 80 | ## Declare things to be passed to dependent projects 81 | ## INCLUDE_DIRS: uncomment this if you package contains header files 82 | ## LIBRARIES: libraries you create in this project that dependent projects also need 83 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 84 | ## DEPENDS: system dependencies of this project that dependent projects also need 85 | catkin_package( 86 | # INCLUDE_DIRS include 87 | # LIBRARIES jir_stereo_pc 88 | # CATKIN_DEPENDS image_transport jir_rectification_remap_lib jir_stereo_util_lib message_filters pcl_conversions pointgrey_disparity_lib roscpp sensor_msgs 89 | # DEPENDS system_lib 90 | ) 91 | 92 | ########### 93 | ## Build ## 94 | ########### 95 | 96 | ## Specify additional locations of header files 97 | ## Your package locations should be listed before other locations 98 | # include_directories(include) 99 | include_directories( 100 | ${catkin_INCLUDE_DIRS} 101 | ) 102 | 103 | ## Declare a cpp library 104 | # add_library(jir_stereo_pc 105 | # src/${PROJECT_NAME}/jir_stereo_pc.cpp 106 | # ) 107 | 108 | ## Declare a cpp executable 109 | add_executable(jir_image_remapper src/node.cpp) 110 | 111 | ## Add cmake target dependencies of the executable/library 112 | ## as an example, message headers may need to be generated before nodes 113 | # add_dependencies(jir_stereo_pc_node jir_stereo_pc_generate_messages_cpp) 114 | 115 | ## Specify libraries to link a library or executable target against 116 | target_link_libraries(jir_image_remapper 117 | ${catkin_LIBRARIES} 118 | ) 119 | 120 | ############# 121 | ## Install ## 122 | ############# 123 | 124 | # all install targets should use catkin DESTINATION variables 125 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 126 | 127 | ## Mark executable scripts (Python etc.) for installation 128 | ## in contrast to setup.py, you can choose the destination 129 | # install(PROGRAMS 130 | # scripts/my_python_script 131 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 132 | # ) 133 | 134 | ## Mark executables and/or libraries for installation 135 | # install(TARGETS jir_stereo_pc jir_stereo_pc_node 136 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 139 | # ) 140 | 141 | ## Mark cpp header files for installation 142 | # install(DIRECTORY include/${PROJECT_NAME}/ 143 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 144 | # FILES_MATCHING PATTERN "*.h" 145 | # PATTERN ".svn" EXCLUDE 146 | # ) 147 | 148 | ## Mark other files for installation (e.g. launch and bag files, etc.) 149 | # install(FILES 150 | # # myfile1 151 | # # myfile2 152 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 153 | # ) 154 | 155 | ############# 156 | ## Testing ## 157 | ############# 158 | 159 | ## Add gtest based cpp test target and link libraries 160 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_jir_stereo_pc.cpp) 161 | # if(TARGET ${PROJECT_NAME}-test) 162 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 163 | # endif() 164 | 165 | ## Add folders to be run by python nosetests 166 | # catkin_add_nosetests(test) 167 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_image_remapper/launch/image_remapper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_image_remapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jir_image_remapper 4 | 0.0.0 5 | The jir_image_remapper package 6 | 7 | 8 | 9 | 10 | Jacobs Robotics 11 | 12 | 13 | 14 | 15 | 16 | CC BY-NC-ND 4.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | image_transport 44 | jir_rectification_remap_lib 45 | message_filters 46 | roscpp 47 | sensor_msgs 48 | cv_bridge 49 | image_transport 50 | jir_rectification_remap_lib 51 | message_filters 52 | roscpp 53 | sensor_msgs 54 | cv_bridge 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_image_remapper/src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | 50 | using namespace std; 51 | using namespace message_filters; 52 | 53 | 54 | jir_rectification_remap_lib::RectificationRemapper remap_left; 55 | 56 | image_transport::CameraPublisher left_rect_pub; 57 | sensor_msgs::CameraInfo left_info; 58 | 59 | cv_bridge::CvImage left_rect; 60 | sensor_msgs::Image rect_msg; 61 | 62 | 63 | void process(const sensor_msgs::Image::ConstPtr& left) { 64 | cv_bridge::CvImageConstPtr cv_left; 65 | try 66 | { 67 | cv_left = cv_bridge::toCvShare(left, sensor_msgs::image_encodings::BGR8 ); 68 | } 69 | catch (cv_bridge::Exception& e) 70 | { 71 | ROS_ERROR("cv_bridge exception: %s", e.what()); 72 | return; 73 | } 74 | 75 | ROS_INFO_STREAM("input image: " << cv_left->image.cols << "x" << cv_left->image.rows); 76 | 77 | //ROS_INFO_STREAM("converted"); 78 | 79 | bool ok = remap_left (cv_left->image , left_rect.image ); 80 | 81 | if(!ok) { 82 | ROS_ERROR_STREAM("error in remap, not right input image size?"); 83 | return; 84 | } 85 | 86 | 87 | left_rect.header = cv_left->header; 88 | left_rect.encoding = sensor_msgs::image_encodings::BGR8; 89 | 90 | left_rect.toImageMsg(rect_msg); 91 | left_info.header = rect_msg.header; 92 | left_rect_pub.publish( rect_msg, left_info); 93 | } 94 | 95 | int main(int argc, char *argv[]) 96 | { 97 | ROS_INFO_STREAM("node sarted"); 98 | ros::init(argc, argv, "jir_image_remapper"); 99 | 100 | ros::NodeHandle nh("~"); 101 | 102 | std::string left_map, right_map; 103 | nh.getParam("left_map", left_map); 104 | 105 | 106 | if(! remap_left.loadMap( left_map ) ) { 107 | ROS_ERROR_STREAM("error loading left map: " << left_map); 108 | return 1; 109 | } 110 | 111 | 112 | left_info = remap_left.getCameraInfo(); 113 | 114 | image_transport::ImageTransport it(nh); 115 | 116 | left_rect_pub = it.advertiseCamera("out_left_rect/image_raw", 1); 117 | 118 | image_transport::Subscriber sub = it.subscribe("in_left", 1, process); 119 | 120 | 121 | 122 | ROS_INFO_STREAM("NODE READY"); 123 | 124 | while(ros::ok()) { 125 | try { 126 | ros::spin(); 127 | } catch ( boost::thread_interrupted& e ) { 128 | ROS_WARN_STREAM("caught thread_interrupted..."); 129 | } 130 | } 131 | 132 | 133 | return 0; 134 | } 135 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_rectification_remap_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jir_rectification_remap_lib) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | cv_bridge 9 | image_transport 10 | roscpp 11 | rospy 12 | sensor_msgs 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 36 | ## pulled in transitively but can be declared for certainty nonetheless: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################### 77 | ## catkin specific configuration ## 78 | ################################### 79 | ## The catkin_package macro generates cmake config files for your package 80 | ## Declare things to be passed to dependent projects 81 | ## INCLUDE_DIRS: uncomment this if you package contains header files 82 | ## LIBRARIES: libraries you create in this project that dependent projects also need 83 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 84 | ## DEPENDS: system dependencies of this project that dependent projects also need 85 | catkin_package( 86 | INCLUDE_DIRS include 87 | LIBRARIES jir_rectification_remap_lib 88 | CATKIN_DEPENDS cv_bridge image_transport roscpp rospy sensor_msgs std_msgs 89 | DEPENDS OpenCV 90 | ) 91 | 92 | ########### 93 | ## Build ## 94 | ########### 95 | 96 | ## Specify additional locations of header files 97 | ## Your package locations should be listed before other locations 98 | # include_directories(include) 99 | include_directories( 100 | include 101 | ${catkin_INCLUDE_DIRS} 102 | ) 103 | 104 | ## Declare a cpp library 105 | add_library(jir_rectification_remap_lib 106 | src/rectification_remapper.cpp 107 | ) 108 | 109 | ## Declare a cpp executable 110 | # add_executable(jir_rectification_remap_lib src/main.cpp src/defractor.cpp) 111 | 112 | ## Add cmake target dependencies of the executable/library 113 | ## as an example, message headers may need to be generated before nodes 114 | # add_dependencies(jir_rectification_remap_lib_node jir_rectification_remap_lib_generate_messages_cpp) 115 | 116 | ## Specify libraries to link a library or executable target against 117 | target_link_libraries(jir_rectification_remap_lib 118 | ${catkin_LIBRARIES} 119 | ) 120 | 121 | ############# 122 | ## Install ## 123 | ############# 124 | 125 | # all install targets should use catkin DESTINATION variables 126 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 127 | 128 | ## Mark executable scripts (Python etc.) for installation 129 | ## in contrast to setup.py, you can choose the destination 130 | # install(PROGRAMS 131 | # scripts/my_python_script 132 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 133 | # ) 134 | 135 | ## Mark executables and/or libraries for installation 136 | # install(TARGETS jir_rectification_remap_lib jir_rectification_remap_lib_node 137 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 139 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 140 | # ) 141 | 142 | ## Mark cpp header files for installation 143 | # install(DIRECTORY include/${PROJECT_NAME}/ 144 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 145 | # FILES_MATCHING PATTERN "*.h" 146 | # PATTERN ".svn" EXCLUDE 147 | # ) 148 | 149 | ## Mark other files for installation (e.g. launch and bag files, etc.) 150 | # install(FILES 151 | # # myfile1 152 | # # myfile2 153 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 154 | # ) 155 | 156 | ############# 157 | ## Testing ## 158 | ############# 159 | 160 | ## Add gtest based cpp test target and link libraries 161 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_jir_rectification_remap_lib.cpp) 162 | # if(TARGET ${PROJECT_NAME}-test) 163 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 164 | # endif() 165 | 166 | ## Add folders to be run by python nosetests 167 | # catkin_add_nosetests(test) 168 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_rectification_remap_lib/include/jir_rectification_remap_lib/rectification_remapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace jir_rectification_remap_lib { 33 | 34 | class RectificationRemapper{ 35 | public: 36 | RectificationRemapper(){}; 37 | ~RectificationRemapper(){}; 38 | 39 | bool loadMap(const std::string& filename); 40 | 41 | cv::Mat getRemappedCameraIntrinsics() const; 42 | cv::Mat getMask() const; 43 | 44 | sensor_msgs::CameraInfo getCameraInfo(); 45 | 46 | bool rectifyImage (const cv::Mat& input, cv::Mat& output); 47 | 48 | inline bool operator()(const cv::Mat& input, cv::Mat& output) { 49 | return rectifyImage(input,output); 50 | } 51 | 52 | 53 | double getFocalLength() const { 54 | return info.K[0]; 55 | } 56 | double getCx() const { 57 | return info.K[2]; 58 | } 59 | double getCy() const { 60 | return info.K[5]; 61 | } 62 | 63 | int expectedRows() const { 64 | return input_rows; 65 | } 66 | int expectedCols() const { 67 | return input_cols; 68 | } 69 | 70 | 71 | private: 72 | cv::Mat mask; 73 | cv::Mat mapx; 74 | cv::Mat mapy; 75 | cv::Mat intrinsics; 76 | int input_rows, input_cols; 77 | sensor_msgs::CameraInfo info; 78 | int H; 79 | int W; 80 | double f; 81 | 82 | }; 83 | 84 | } 85 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_rectification_remap_lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jir_rectification_remap_lib 4 | 0.0.0 5 | The jir_rectification_remap_lib package 6 | 7 | 8 | 9 | 10 | Jacobs Robotics 11 | 12 | 13 | 14 | 15 | 16 | CC BY-NC-ND 4.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_bridge 44 | image_transport 45 | roscpp 46 | rospy 47 | sensor_msgs 48 | std_msgs 49 | cv_bridge 50 | image_transport 51 | roscpp 52 | rospy 53 | sensor_msgs 54 | std_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_rectification_remap_lib/src/rectification_remapper.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Jacobs University Robotics Group 3 | * All rights reserved. 4 | * 5 | * 6 | * Unless specified otherwise this code examples are released under 7 | * Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | * Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | * 10 | * 11 | * If you are interested in using this code commercially, 12 | * please contact us. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | * Contact: robotics@jacobs-university.de 26 | */ 27 | 28 | #include "jir_rectification_remap_lib/rectification_remapper.h" 29 | 30 | #include 31 | #include 32 | 33 | 34 | 35 | using namespace std; 36 | using namespace cv; 37 | 38 | namespace jir_rectification_remap_lib { 39 | 40 | bool RectificationRemapper::loadMap(const std::string& filename){ 41 | cv::FileStorage fs(filename, cv::FileStorage::READ); 42 | 43 | if( !fs.isOpened() ) { 44 | return false; 45 | } 46 | 47 | fs["mapx"] >> mapx; 48 | fs["mapy"] >> mapy; 49 | fs["mask"] >> mask; 50 | Mat K; 51 | fs["camera_info"] >> K; 52 | intrinsics=K; 53 | 54 | cv::Size s = mapx.size(); 55 | H = s.height; 56 | W = s.width; 57 | 58 | info.height=H; 59 | info.width=W; 60 | 61 | info.K[0]=K.at(0,0); 62 | info.K[1]=0; 63 | info.K[2]=K.at(0,2); 64 | 65 | info.K[3]=0; 66 | info.K[4]=K.at(1,1); 67 | info.K[5]=K.at(1,2); 68 | 69 | info.K[6]=0; 70 | info.K[7]=0; 71 | info.K[8]=1; 72 | 73 | info.R[0] = info.R[4] = info.R[8] = 1.0; 74 | 75 | info.P[0] = K.at(0,0); 76 | info.P[2] = K.at(0,2); 77 | info.P[5] = K.at(1,1); 78 | info.P[6] = K.at(1,2); 79 | info.P[10] = 1.0; 80 | 81 | info.binning_x = 0; 82 | info.binning_y = 0; 83 | info.roi.height = H; 84 | info.roi.width = W; 85 | info.roi.do_rectify = false; 86 | 87 | fs["input_rows"] >> input_rows; 88 | fs["input_cols"] >> input_cols; 89 | 90 | return true; 91 | } 92 | 93 | Mat RectificationRemapper::getRemappedCameraIntrinsics() const { 94 | return intrinsics; 95 | } 96 | 97 | Mat RectificationRemapper::getMask() const { 98 | return mask; 99 | } 100 | 101 | sensor_msgs::CameraInfo RectificationRemapper::getCameraInfo(){ 102 | return info; 103 | } 104 | 105 | bool RectificationRemapper::rectifyImage (const cv::Mat& input, cv::Mat& output){ 106 | if( input.rows != input_rows || input.cols != input_cols ) { 107 | return false; 108 | } 109 | 110 | remap(input, output, mapx, mapy, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar::all(0)); 111 | 112 | return true; 113 | } 114 | 115 | } 116 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jir_refractive_image_geometry) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | jir_refractive_image_geometry_msgs 9 | image_geometry 10 | roscpp 11 | tf 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | find_package(eigen REQUIRED) 17 | 18 | find_package( PkgConfig REQUIRED) 19 | pkg_check_modules( gsl REQUIRED gsl ) 20 | 21 | find_package(OpenCV REQUIRED) 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 39 | ## pulled in transitively but can be declared for certainty nonetheless: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # std_msgs # Or other packages containing msgs 77 | # ) 78 | 79 | ################################### 80 | ## catkin specific configuration ## 81 | ################################### 82 | ## The catkin_package macro generates cmake config files for your package 83 | ## Declare things to be passed to dependent projects 84 | ## INCLUDE_DIRS: uncomment this if you package contains header files 85 | ## LIBRARIES: libraries you create in this project that dependent projects also need 86 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 87 | ## DEPENDS: system dependencies of this project that dependent projects also need 88 | catkin_package( 89 | INCLUDE_DIRS include 90 | # LIBRARIES jir_refractive_image_geometry 91 | CATKIN_DEPENDS image_geometry jir_refractive_image_geometry_msgs 92 | DEPENDS eigen gsl 93 | ) 94 | 95 | ########### 96 | ## Build ## 97 | ########### 98 | 99 | ## Specify additional locations of header files 100 | ## Your package locations should be listed before other locations 101 | # include_directories(include) 102 | include_directories( 103 | ${catkin_INCLUDE_DIRS} 104 | ${OpenCV_INCLUDE_DIRS} 105 | include 106 | ) 107 | 108 | 109 | 110 | ############# 111 | ## Install ## 112 | ############# 113 | 114 | # all install targets should use catkin DESTINATION variables 115 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 116 | 117 | ## Mark executable scripts (Python etc.) for installation 118 | ## in contrast to setup.py, you can choose the destination 119 | # install(PROGRAMS 120 | # scripts/my_python_script 121 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 122 | # ) 123 | 124 | ## Mark executables and/or libraries for installation 125 | # install(TARGETS jir_refractive_image_geometry jir_refractive_image_geometry_node 126 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark cpp header files for installation 132 | # install(DIRECTORY include/${PROJECT_NAME}/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # FILES_MATCHING PATTERN "*.h" 135 | # PATTERN ".svn" EXCLUDE 136 | # ) 137 | 138 | ## Mark other files for installation (e.g. launch and bag files, etc.) 139 | # install(FILES 140 | # # myfile1 141 | # # myfile2 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | # ) 144 | 145 | ############# 146 | ## Testing ## 147 | ############# 148 | 149 | ## Add gtest based cpp test target and link libraries 150 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_jir_refractive_image_geometry.cpp) 151 | # if(TARGET ${PROJECT_NAME}-test) 152 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 153 | # endif() 154 | 155 | ## Add folders to be run by python nosetests 156 | # catkin_add_nosetests(test) 157 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/include/jir_refractive_image_geometry/pinhole_camera_model.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace jir_refractive_image_geometry { 13 | 14 | class Exception : public std::runtime_error 15 | { 16 | public: 17 | Exception(const std::string& description) : std::runtime_error(description) {} 18 | }; 19 | 20 | enum DistortionState { NONE, CALIBRATED, UNKNOWN }; 21 | 22 | 23 | template < typename Derived > 24 | class PinholeCameraModelParametersBase { 25 | public: 26 | typedef typename Derived::Scalar Scalar; 27 | 28 | size_t numberOfDistortionCoeffs() const { return Derived::numberOfDistortionCoeffs(); } 29 | 30 | /// Focal length in x 31 | const Scalar& fx() const { return Derived::fx(); } 32 | /// Focal length in y 33 | const Scalar& fy() const { return Derived::fy(); } 34 | /// Principal point in x 35 | const Scalar& cx() const { return Derived::cx(); } 36 | /// Principal point in y 37 | const Scalar& cy() const { return Derived::cy(); } 38 | /// Distortion Parameters 39 | const Scalar& distortionCoeff(size_t i) const { return Derived::distortionCoeff(i); } 40 | 41 | /// Focal length in x 42 | Scalar& fx() { return Derived::fx(); } 43 | /// Focal length in y 44 | Scalar& fy() { return Derived::fy(); } 45 | /// Principal point in x 46 | Scalar& cx() { return Derived::cx(); } 47 | /// Principal point in y 48 | Scalar& cy() { return Derived::cy(); } 49 | /// Distortion Parameters 50 | Scalar& distortionCoeff(size_t i) { return Derived::distortionCoeff(i); } 51 | }; 52 | 53 | template < typename Scalar_ > 54 | class PlumbBobPinholeCameraModelParametersVector : public PinholeCameraModelParametersBase< PinholeCameraModelParametersPinholeCameraModelParametersVector { 55 | public: 56 | typedef Scalar_ Scalar; 57 | typedef Eigen::Matrix ParameterVector; 58 | 59 | ParameterVector values; 60 | 61 | size_t length() const { return 4+num_distortion_coeffs; } 62 | Scalar* data() { return values.data(); } 63 | 64 | PinholeCameraModelParameters() 65 | } 66 | 67 | template < typename Scalar > 68 | class PinholeCameraModel { 69 | public: 70 | typedef Eigen::Matrix Vector; 71 | typedef Eigen::Matrix Matrix33; 72 | typedef Eigen::Matrix Matrix34; 73 | 74 | protected: 75 | 76 | 77 | Vector D_; // Unaffected by binning, ROI 78 | Matrix33 R_; // Unaffected by binning, ROI 79 | Matrix33 K_; // Describe current image (includes binning, ROI) 80 | Matrix34 P_; // Describe current image (includes binning, ROI) 81 | Matrix33 K_full_; // Describe full-res image, needed for full maps 82 | Matrix34 P_full_; // Describe full-res image, needed for full maps 83 | 84 | struct CalibCache { 85 | 86 | }; 87 | 88 | 89 | sensor_msgs::CameraInfo cam_info_; 90 | public: 91 | virtual ~PinholeCameraModel() {} 92 | 93 | PinholeCameraModel() {} 94 | 95 | /** 96 | * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. 97 | */ 98 | bool fromCameraInfo(const sensor_msgs::CameraInfo& msg) { 99 | 100 | } 101 | 102 | /** 103 | * \brief Set the camera parameters from the sensor_msgs/CameraInfo message. 104 | */ 105 | bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg) { 106 | 107 | } 108 | 109 | /** 110 | * \brief Returns true if the camera has been initialized 111 | */ 112 | bool initialized() const { return (bool)cache_; } 113 | 114 | 115 | 116 | /* Trivial inline functions */ 117 | inline std::string tfFrame() const 118 | { 119 | assert( initialized() ); 120 | return cam_info_.header.frame_id; 121 | } 122 | 123 | inline ros::Time stamp() const 124 | { 125 | assert( initialized() ); 126 | return cam_info_.header.stamp; 127 | } 128 | 129 | inline const sensor_msgs::CameraInfo& cameraInfo() const { return cam_info_; } 130 | inline const Matrix33& intrinsicMatrix() const { return K_; } 131 | inline const Vector& distortionCoeffs() const { return D_; } 132 | inline const Matrix33& rotationMatrix() const { return R_; } 133 | inline const Matrix34& projectionMatrix() const { return P_; } 134 | inline const Matrix33& fullIntrinsicMatrix() const { return K_full_; } 135 | inline const Matrix34& fullProjectionMatrix() const { return P_full_; } 136 | 137 | inline Scalar fx() const { return P_(0,0); } 138 | inline Scalar fy() const { return P_(1,1); } 139 | inline Scalar cx() const { return P_(0,2); } 140 | inline Scalar cy() const { return P_(1,2); } 141 | inline Scalar Tx() const { return P_(0,3); } 142 | inline Scalar Ty() const { return P_(1,3); } 143 | 144 | /** 145 | * \brief Returns the number of columns in each bin. 146 | */ 147 | inline uint32_t binningX() const { return cam_info_.binning_x; } 148 | /** 149 | * \brief Returns the number of rows in each bin. 150 | */ 151 | inline uint32_t binningY() const { return cam_info_.binning_y; } 152 | 153 | /** 154 | * \brief Compute delta u, given Z and delta X in Cartesian space. 155 | * 156 | * For given Z, this is the inverse of getDeltaX(). 157 | * 158 | * \param deltaX Delta X, in Cartesian space 159 | * \param Z Z (depth), in Cartesian space 160 | */ 161 | inline Scalar getDeltaU(Scalar deltaX, Scalar Z) const 162 | { 163 | assert( initialized() ); 164 | return fx() * deltaX / Z; 165 | } 166 | 167 | /** 168 | * \brief Compute delta v, given Z and delta Y in Cartesian space. 169 | * 170 | * For given Z, this is the inverse of getDeltaY(). 171 | * 172 | * \param deltaY Delta Y, in Cartesian space 173 | * \param Z Z (depth), in Cartesian space 174 | */ 175 | inline Scalar getDeltaV(Scalar deltaY, Scalar Z) const 176 | { 177 | assert( initialized() ); 178 | return fy() * deltaY / Z; 179 | } 180 | 181 | /** 182 | * \brief Compute delta X, given Z in Cartesian space and delta u in pixels. 183 | * 184 | * For given Z, this is the inverse of getDeltaU(). 185 | * 186 | * \param deltaU Delta u, in pixels 187 | * \param Z Z (depth), in Cartesian space 188 | */ 189 | inline Scalar getDeltaX(Scalar deltaU, Scalar Z) const 190 | { 191 | assert( initialized() ); 192 | return Z * deltaU / fx(); 193 | } 194 | 195 | /** 196 | * \brief Compute delta Y, given Z in Cartesian space and delta v in pixels. 197 | * 198 | * For given Z, this is the inverse of getDeltaV(). 199 | * 200 | * \param deltaV Delta v, in pixels 201 | * \param Z Z (depth), in Cartesian space 202 | */ 203 | inline Scalar getDeltaY(Scalar deltaV, Scalar Z) const 204 | { 205 | assert( initialized() ); 206 | return Z * deltaV / fy(); 207 | } 208 | }; 209 | 210 | 211 | } 212 | 213 | #endif // JIR_REFRACTIVE_IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP 214 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/include/jir_refractive_image_geometry/polynomial.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY__POLYNOMIAL_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY__POLYNOMIAL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace jir_refractive_image_geometry { 9 | using namespace std; 10 | 11 | template < typename Scalar > 12 | vector cubic(Scalar x3, Scalar x2, Scalar x1, Scalar x0) { 13 | vector v; 14 | 15 | v.push_back(x0); 16 | v.push_back(x1); 17 | v.push_back(x2); 18 | v.push_back(x3); 19 | 20 | return v; 21 | } 22 | 23 | template < typename Scalar > 24 | vector quadratic(Scalar x2, Scalar x1, Scalar x0) { 25 | vector v; 26 | 27 | v.push_back(x0); 28 | v.push_back(x1); 29 | v.push_back(x2); 30 | 31 | return v; 32 | } 33 | 34 | template < typename Scalar > 35 | vector linear(Scalar x1, Scalar x0) { 36 | vector v; 37 | 38 | v.push_back(x0); 39 | v.push_back(x1); 40 | 41 | return v; 42 | } 43 | 44 | template < typename Scalar > 45 | vector add(const vector& v1, const vector& v2) { 46 | vector ret( max(v1.size(), v2.size() ), Scalar(0.) ); 47 | for(size_t i=0; i 57 | vector sub(const vector& v1, const vector& v2) { 58 | vector ret( max(v1.size(), v2.size() ), Scalar(0.) ); 59 | for(size_t i=0; i 69 | size_t degree(const vector& v) { 70 | Scalar zero(0); 71 | 72 | size_t deg = v.size()-1; 73 | while(v[deg] == zero && deg > 0) { 74 | deg--; 75 | } 76 | 77 | return deg; 78 | } 79 | 80 | template < typename Scalar > 81 | string pretty_string(const vector& v, const std::string& var) { 82 | ostringstream out; 83 | pretty_print(out, v, var); 84 | return out.str(); 85 | } 86 | 87 | template < typename Scalar > 88 | void pretty_print(std::ostream& out, const vector& v, const std::string& var) { 89 | bool first = true; 90 | Scalar zero(0); 91 | 92 | for(size_t i=v.size(); i>0; i--) { // because size_t is unsigned, and after 0 comes max 93 | if( v[i-1] == zero ) continue; 94 | 95 | if( !first ) { 96 | out << " "; 97 | if( v[i-1] >= zero) { 98 | out << "+"; 99 | } 100 | } 101 | out << v[i-1]; 102 | if( i > 1 ) { 103 | out << " " << var << "^{" << (i-1) << "}"; 104 | } 105 | first = false; 106 | } 107 | } 108 | 109 | template < typename Scalar > 110 | vector mult(Scalar s, const vector& v) { 111 | vector ret(v); 112 | for(size_t i=0; i 120 | vector mult(const vector& v1, const vector& v2) { 121 | size_t new_degree = degree(v1)+degree(v2); 122 | //cout << "new deg: " << new_degree << endl; 123 | 124 | vector ret(new_degree+1,Scalar(0)); 125 | 126 | for(size_t i=0; i out; 12 | 13 | out.offset = tr_in_out * r_in.offset; 14 | out.direction = tr_in_out.rotation() * r_in.direction; 15 | 16 | out.frame_id = new_frame_id; 17 | 18 | return out; 19 | } 20 | 21 | template < typename Scalar, typename Derived1, typename Derived3 > 22 | bool intersect(const Ray& r, const Eigen::MatrixBase& plane_normal, const Scalar& plane_distance, Eigen::MatrixBase& point) { 23 | Scalar nd = plane_normal.dot( r.direction ); 24 | if(std::fabs(nd) < 1e-10) return false; 25 | 26 | Scalar t = ( -plane_distance - plane_normal.dot( r.offset ) ) / nd; 27 | 28 | if( t < 1e-10 ) return false; 29 | 30 | point = r.offset + t*r.direction; 31 | return true; 32 | } 33 | 34 | template < typename Scalar, typename Derived1, typename Derived2 > 35 | void closest_points(const Ray& r1, const Ray& r2, MatrixBase& p_on_r1, MatrixBase& p_on_r2) { 36 | Matrix m = r1.offset - r2.offset; 37 | 38 | Scalar d11 = r1.direction.dot(r1.direction); 39 | Scalar d12 = r1.direction.dot(r2.direction); 40 | Scalar d22 = r2.direction.dot(r2.direction); 41 | Scalar d1m = r1.direction.dot(m); 42 | Scalar d2m = r2.direction.dot(m); 43 | 44 | Scalar d = d11*d22 - d12*d12; 45 | Scalar x1 = (d12*d2m - d22*d1m)/d; 46 | Scalar x2 = (d11*d2m - d12*d1m)/d; 47 | 48 | p_on_r1 = r1.offset; 49 | p_on_r2 = r2.offset; 50 | 51 | if( x1 > 0 ) p_on_r1 += x1*r1.direction; 52 | if( x2 > 0 ) p_on_r2 += x2*r2.direction; 53 | } 54 | 55 | template < typename Derived > 56 | void triangulate(const Ray< typename internal::traits::Scalar >& r1, const Ray< typename internal::traits::Scalar >& r2, MatrixBase& p) { 57 | Matrix< typename internal::traits::Scalar ,3,1> p1, p2; 58 | closest_points(r1,r2,p1,p2); 59 | p = (p1+p2)/( typename internal::traits::Scalar(2) ); 60 | } 61 | 62 | template < typename Scalar > 63 | void distance(const Ray& r1, const Ray& r2, Scalar& dist) { 64 | Matrix p1, p2; 65 | closest_points(r1,r2,p1,p2); 66 | dist = (p1-p2).norm(); 67 | } 68 | 69 | template < typename Derived > 70 | void distance(const Ray< typename internal::traits::Scalar >& r, const MatrixBase& p, typename internal::traits::Scalar& dist) { 71 | typename internal::traits::Scalar len = (p-r.offset).dot(r.direction); 72 | if( len > typename internal::traits::Scalar(0) ) { 73 | dist = (p - (len*r.direction + r.offset)).norm(); 74 | } else { 75 | dist = (p-r.offset).norm(); 76 | } 77 | } 78 | 79 | template < typename Derived > 80 | void square_distance(const Ray< typename internal::traits::Scalar >& r, const MatrixBase& p, typename internal::traits::Scalar& dist) { 81 | typename internal::traits::Scalar len = (p-r.offset).dot(r.direction); 82 | if( len > typename internal::traits::Scalar(0) ) { 83 | dist = (p - (len*r.direction + r.offset)).squaredNorm(); 84 | } else { 85 | dist = (p-r.offset).squaredNorm(); 86 | } 87 | } 88 | 89 | } 90 | 91 | #endif // JIR_REFRACTIVE_IMAGE_GEOMETRY__RAY_OPERATIONS_HPP 92 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/include/jir_refractive_image_geometry/refracted_pinhole_camera_model.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTED_PINHOLE_CAMERA_MODEL_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTED_PINHOLE_CAMERA_MODEL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace jir_refractive_image_geometry { 15 | 16 | class RefractedPinholeCameraModel : public image_geometry::PinholeCameraModel { 17 | public: 18 | RefractedPinholeCameraModel() : image_geometry::PinholeCameraModel(), planarRefractionInfoIsSet(false) {} 19 | 20 | inline bool fromPlanarRefractionInfo( const jir_refractive_image_geometry_msgs::PlanarRefractionInfoConstPtr& info) { 21 | return fromPlanarRefractionInfo(*info); 22 | } 23 | inline bool fromPlanarRefractionInfo( const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& info) { 24 | planarRefractionInfoIsSet = true; 25 | planarRefractionInfo = info; 26 | } 27 | 28 | template < typename Derived > 29 | Ray< typename Eigen::internal::traits::Scalar > projectPixelTo3dRay(const Eigen::MatrixBase& p) const { 30 | return projectPixelTo3dRay< typename Eigen::internal::traits::Scalar >(p(0), p(1)); 31 | } 32 | 33 | template < typename Scalar > 34 | Ray projectPixelTo3dRay(Scalar u_rectified, Scalar v_rectified) const 35 | { 36 | Scalar r0_x = (u_rectified - Scalar(cx()) - Scalar(Tx()) ) / Scalar(fx()); 37 | Scalar r0_y = (v_rectified - Scalar(cy()) - Scalar(Ty()) ) / Scalar(fy()); 38 | 39 | Ray r_out(cam_info_.header.frame_id, r0_x, r0_y, Scalar(1)); 40 | 41 | // maybe transform ray into refraction info frame 42 | 43 | if( planarRefractionInfoIsSet ) 44 | { 45 | Ray r0 = r_out; 46 | refract(r0, planarRefractionInfo, r_out); 47 | } 48 | 49 | return r_out; 50 | } 51 | 52 | template < typename Derived > 53 | Eigen::Matrix< typename Eigen::internal::traits::Scalar, 2, 1 > project3dToPixel(const Eigen::MatrixBase& xyz_) const { 54 | typedef typename Eigen::internal::traits::Scalar Scalar; 55 | 56 | Matrix xyz = xyz_; 57 | 58 | if( planarRefractionInfoIsSet ) 59 | { 60 | // find point on inside of refraction interface corresponding to beam 61 | Matrix xyz_in = xyz; 62 | point_on_inside_of_refractive_plane_analytic(xyz_in, planarRefractionInfo, xyz); // tolerance 1e-10, max 100 iterations 63 | } 64 | 65 | Matrix< Scalar, 2, 1 > p; 66 | p(0) = ( Scalar(fx()) * xyz(0) + Scalar(Tx()) ) / xyz(2) + Scalar(cx()); 67 | p(1) = ( Scalar(fy()) * xyz(1) + Scalar(Ty()) ) / xyz(2) + Scalar(cy()); 68 | 69 | return p; 70 | } 71 | 72 | 73 | protected: 74 | jir_refractive_image_geometry_msgs::PlanarRefractionInfo planarRefractionInfo; 75 | bool planarRefractionInfoIsSet; 76 | 77 | 78 | // struct gsl_roots_workspace; 79 | // boost::shared_ptr gsl_roots; 80 | }; 81 | 82 | } 83 | 84 | #endif // JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTED_PINHOLE_CAMERA_MODEL_HPP 85 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/include/jir_refractive_image_geometry/refraction_operations.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTION_OPERATIONS_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTION_OPERATIONS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace jir_refractive_image_geometry { 13 | using namespace Eigen; 14 | using namespace std; 15 | 16 | template < typename Derived > 17 | void refract_at_interface(const MatrixBase& l, const MatrixBase& n, const typename internal::traits::Scalar& n1, const typename internal::traits::Scalar& n2, MatrixBase& out_v) { 18 | typedef typename internal::traits::Scalar Scalar; 19 | 20 | // from Wikipedia 21 | 22 | Scalar c = -n.dot(l); 23 | Scalar r = n1/n2; 24 | 25 | Scalar s = Scalar(1.0) - r*r * ( Scalar(1.0) - c*c ); 26 | 27 | if( s > Scalar(0.0) ) { // (partial) refraction 28 | out_v = r * l + (r*c - sqrt(s)) * n; 29 | } else { // total reflection 30 | out_v = l + Scalar(2.0) * c * n; 31 | } 32 | } 33 | 34 | template< typename Scalar > 35 | void refract(const Ray& input, const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& params, Ray& output) { 36 | if( input.frame_id != params.header.frame_id ) { 37 | throw std::runtime_error("Ray and PlanarRefractionInfo are not in the same coordinate frame!"); 38 | } 39 | 40 | output = input; 41 | Matrix plane_normal( Scalar(params.normal[0]), Scalar(params.normal[1]), Scalar(params.normal[2]) ); 42 | Scalar plane_d_0( params.d_0 ); 43 | Scalar plane_d_1 = Scalar(params.d_0) + Scalar(params.d_1); 44 | 45 | Scalar mu1 = Scalar(params.n_glass); 46 | Scalar mu2 = Scalar(params.n_water); 47 | 48 | 49 | Matrix new_offset; 50 | if( intersect(output, plane_normal, plane_d_0, new_offset)) { 51 | output.offset = new_offset; 52 | 53 | refract_at_interface(output.direction, plane_normal, 1, mu1, output.direction); 54 | } 55 | 56 | if( intersect(output, plane_normal, plane_d_1, new_offset)) { 57 | output.offset = new_offset; 58 | 59 | refract_at_interface(output.direction, plane_normal, mu1, mu2, output.direction); 60 | } 61 | 62 | } 63 | 64 | template< typename Derived > 65 | void point_on_inside_of_refractive_plane_analytic(const MatrixBase& in_outside_point, const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& params, MatrixBase& out_inside_point) { 66 | typedef typename internal::traits::Scalar Scalar; 67 | 68 | Matrix plane_normal( Scalar(params.normal[0]), Scalar(params.normal[1]), Scalar(params.normal[2]) ); 69 | Scalar plane_d_0( params.d_0 ); 70 | Scalar plane_d_1( params.d_1 ); 71 | 72 | //plane_d_1 += plane_d_0; 73 | 74 | Scalar mu1 = Scalar(params.n_glass); 75 | Scalar mu2 = Scalar(params.n_water); 76 | 77 | //plane_normal = -plane_normal; 78 | 79 | Matrix POR = plane_normal.cross(in_outside_point).normalized(); 80 | 81 | Matrix z1 = -plane_normal; 82 | Matrix z2 = POR.cross(z1).normalized(); 83 | 84 | Scalar u_y = in_outside_point.transpose() * z1; 85 | Scalar u_x = in_outside_point.transpose() * z2; 86 | 87 | /* 88 | cout << "POR: " << POR.transpose() << endl; 89 | cout << "z1: " << z1.transpose() << endl; 90 | cout << "z2: " << z2.transpose() << endl; 91 | cout << "v: " << u_y << endl; 92 | cout << "u: " << u_x << endl; 93 | //*/ 94 | 95 | // build poly, from supplementary.pdf 96 | vector k1 = linear( plane_d_0+plane_d_1-u_y, 0 ); 97 | vector k2 = linear( -1, u_x ); 98 | vector k3 = linear( -plane_d_1, 0 ); 99 | vector D1 = quadratic( mu1*mu1-1, 0, plane_d_0*plane_d_0 * mu1*mu1 ); 100 | vector D2 = quadratic( mu2*mu2-1, 0, plane_d_0*plane_d_0 * mu2*mu2 ); 101 | 102 | vector t1 = pow( 103 | sub( 104 | add( 105 | mult( 106 | pow(k1,2), 107 | D1 108 | ), 109 | mult( 110 | pow(k3,2), 111 | D2 112 | ) 113 | ), 114 | mult( 115 | mult( 116 | pow(k2,2), 117 | D1 118 | ), 119 | D2 120 | ) 121 | ) 122 | , 2) ; 123 | vector t2 = mult( 124 | Scalar(4), 125 | mult( 126 | mult( 127 | pow(k1,2), 128 | pow(k3,2) 129 | ), 130 | mult( 131 | D1, 132 | D2 133 | ) 134 | ) 135 | ); 136 | 137 | vector p = sub( t1, t2 ); 138 | 139 | /* 140 | cout << "degree of p: " << degree(p) << endl; 141 | 142 | cout << endl << endl; 143 | cout << "P:" << endl; 144 | for(size_t i=p.size(); i>0; i--) { 145 | cout << "x^" << i-1 << " -> " << p[i-1] << endl; 146 | } 147 | cout << endl << endl; 148 | //*/ 149 | 150 | vector roots = real_roots(p); 151 | 152 | //cout << "found " << roots.size() << " roots!" << endl; 153 | 154 | 155 | Scalar error(1e100); 156 | for(size_t i=0; i candidate = roots[i] * z2 + plane_d_0 * z1; 160 | 161 | //cout << "Candiate: " << candidate.normalized().transpose() << endl; 162 | 163 | Ray r_inside, r_outside; 164 | r_inside.frame_id = params.header.frame_id; 165 | r_inside.direction = candidate.normalized(); 166 | 167 | refract(r_inside, params, r_outside); 168 | 169 | Scalar dist; 170 | square_distance(r_outside, in_outside_point, dist); 171 | 172 | //cout << "ERROR: " << dist << endl; 173 | 174 | if( error > dist ) { 175 | error = dist; 176 | out_inside_point = candidate; 177 | } 178 | } 179 | 180 | } 181 | 182 | 183 | namespace point_on_inside_of_refractive_plane_iterative__DETAIL { 184 | 185 | template < typename Derived > 186 | typename internal::traits::Scalar distance(typename internal::traits::Scalar x, const MatrixBase& p, const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& params) { 187 | typedef typename internal::traits::Scalar Scalar; 188 | 189 | Matrix plane_normal( Scalar(params.normal[0]), Scalar(params.normal[1]), Scalar(params.normal[2]) ); 190 | Scalar plane_d_0( params.d_0 ); 191 | Scalar plane_d_1( params.d_1 ); 192 | 193 | 194 | Scalar mu1 = Scalar(params.n_glass); 195 | Scalar mu2 = Scalar(params.n_water); 196 | 197 | //plane_normal = -plane_normal; 198 | 199 | Matrix POR = plane_normal.cross(p).normalized(); 200 | 201 | if( abs(plane_normal.dot(p.normalized())) > (1.0-1e-8) ) { 202 | // plane normal and p are parallel, choose any POR 203 | POR[0]=1; POR[1]=1; 204 | POR[2] = -(plane_normal[0]+plane_normal[1])/plane_normal[2]; 205 | POR.normalize(); 206 | 207 | //cout << "Check: " << POR.dot(plane_normal) << endl; 208 | } 209 | 210 | Matrix z1 = -plane_normal; 211 | Matrix z2 = POR.cross(z1).normalized(); 212 | 213 | Ray r_inside; 214 | r_inside.frame_id = params.header.frame_id; 215 | r_inside.direction = x*z2 + plane_d_0*z1; 216 | r_inside.direction.normalize(); 217 | 218 | 219 | Ray r_outside; 220 | refract(r_inside, params, r_outside); 221 | 222 | Scalar dist; 223 | square_distance(r_outside, p, dist); 224 | return dist; 225 | } 226 | 227 | template < typename Derived > 228 | void write_3d_point(typename internal::traits::Scalar x, const MatrixBase& in_outside_point, const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& params, MatrixBase& out_inside_point) { 229 | typedef typename internal::traits::Scalar Scalar; 230 | 231 | Matrix plane_normal( Scalar(params.normal[0]), Scalar(params.normal[1]), Scalar(params.normal[2]) ); 232 | Scalar plane_d_0( params.d_0 ); 233 | Scalar plane_d_1( params.d_1 ); 234 | 235 | plane_d_1 += plane_d_0; 236 | 237 | Scalar mu1 = Scalar(params.n_glass); 238 | Scalar mu2 = Scalar(params.n_water); 239 | 240 | //plane_normal = -plane_normal; 241 | 242 | Matrix POR = plane_normal.cross(in_outside_point).normalized(); 243 | 244 | if( abs(plane_normal.dot(in_outside_point.normalized())) > (1.0-1e-9) ) { 245 | // plane normal and p are parallel, choose any POR 246 | POR[0]=1; POR[1]=1; 247 | POR[2] = -(plane_normal[0]+plane_normal[1])/plane_normal[2]; 248 | POR.normalize(); 249 | 250 | //cout << "Check: " << POR.dot(plane_normal) << endl; 251 | } 252 | 253 | Matrix z1 = -plane_normal; 254 | Matrix z2 = POR.cross(z1).normalized(); 255 | 256 | out_inside_point = x*z2 + plane_d_0*z1; 257 | } 258 | 259 | } 260 | 261 | template< typename Derived > 262 | typename boost::enable_if< boost::is_same< typename internal::traits::Scalar, double>, void>::type point_on_inside_of_refractive_plane_iterative(const MatrixBase& in_outside_point, const jir_refractive_image_geometry_msgs::PlanarRefractionInfo& params, MatrixBase& out_inside_point, typename internal::traits::Scalar tol = 1e-10, size_t max_iterations = 100) { 263 | using namespace point_on_inside_of_refractive_plane_iterative__DETAIL; 264 | typedef typename internal::traits::Scalar Scalar; 265 | 266 | 267 | Matrix plane_normal( Scalar(params.normal[0]), Scalar(params.normal[1]), Scalar(params.normal[2]) ); 268 | if( abs(plane_normal.dot(in_outside_point.normalized())) < 1e-9 ) { 269 | // orthogonal, does not intersect refraction plane! 270 | out_inside_point = in_outside_point; 271 | return; 272 | } 273 | 274 | 275 | 276 | Scalar x(0); 277 | Scalar h(1e-4); 278 | 279 | Scalar f = distance(x, in_outside_point, params); 280 | 281 | cout << "f("<< x << ") = " << f << endl; 282 | 283 | if( f > tol ) { 284 | for( size_t i=0; i < max_iterations; i++ ) { 285 | Scalar f_ph = distance(x+h, in_outside_point, params); 286 | //Scalar f_p2h = distance(x+2*h, in_outside_point, params); 287 | Scalar f_mh = distance(x-h, in_outside_point, params); 288 | //Scalar f_m2h = distance(x-2*h, in_outside_point, params); 289 | 290 | 291 | /* 292 | Scalar f_p = ( 1./12. * f_m2h - 2./3. * f_mh + 2./3. * f_ph -1./12. * f_p2h ) / (h); 293 | Scalar f_pp = (-1./12. * f_m2h + 4./3. * f_mh -5./2. * f + 4./3. * f_ph -1./12. * f_p2h ) / (h*h); 294 | //*/ 295 | 296 | //* 2 accuracy 297 | Scalar f_p = (-.5 * f_mh + .5 * f_ph ) / (h); 298 | Scalar f_pp = ( f_mh -2. * f + f_ph ) / (h*h); 299 | //Scalar f_ppp = (-.5*f_m2h + f_mh - f_ph + .5*f_p2h ) / (h*h*h); 300 | //*/ 301 | 302 | cout << "f_p: " << f_p << endl; 303 | cout << "f_pp: " << f_pp << endl; 304 | //cout << "f_ppp: " << f_ppp << endl; 305 | 306 | if( f_pp <= 0) { 307 | x += -f/f_p; 308 | } else { 309 | x += -f_p/f_pp; 310 | } 311 | 312 | 313 | f = distance(x, in_outside_point, params); 314 | 315 | cout << "f("<< x << ") = " << f << endl; 316 | 317 | if( f < tol ) { 318 | cout << "needed " << i << " iterations" << endl; 319 | break; 320 | } 321 | } 322 | } 323 | 324 | write_3d_point(x, in_outside_point, params, out_inside_point); 325 | } 326 | 327 | } 328 | 329 | #endif // JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTION_OPERATIONS_HPP 330 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/include/jir_refractive_image_geometry/two_layer_refraction.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTIVE_PLANE_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTIVE_PLANE_HPP 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace Eigen { 9 | namespace internal { 10 | 11 | template 12 | struct traits > { 13 | typedef _Scalar Scalar; 14 | typedef Matrix ParametersType; 15 | }; 16 | 17 | template 18 | struct traits, _Options> > 19 | : traits > { 20 | typedef _Scalar Scalar; 21 | typedef Map,_Options> ParametersType; 22 | }; 23 | 24 | template 25 | struct traits, _Options> > 26 | : traits > { 27 | typedef _Scalar Scalar; 28 | typedef Map,_Options> ParametersType; 29 | }; 30 | 31 | } 32 | } 33 | 34 | namespace jir_refractive_image_geometry { 35 | using namespace Eigen; 36 | 37 | template < typename Derived > 38 | class TwoLayerRefractionBase { 39 | public: 40 | typedef typename internal::traits::Scalar Scalar; 41 | 42 | typedef typename internal::traits::ParametersType & ParametersReference; 43 | typedef const typename internal::traits::ParametersType & ConstParametersReference; 44 | 45 | typedef Matrix Vector; 46 | 47 | std::string frame_id; 48 | 49 | Vector normal() const { 50 | return Derived::normal(); 51 | } 52 | 53 | 54 | 55 | }; 56 | 57 | } 58 | 59 | #endif //JIR_REFRACTIVE_IMAGE_GEOMETRY__REFRACTIVE_PLANE_HPP 60 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jir_refractive_image_geometry 4 | 0.0.0 5 | The jir_refractive_image_geometry package 6 | 7 | 8 | 9 | 10 | Jacobs Robotics 11 | 12 | 13 | 14 | 15 | 16 | CC BY-NC-ND 4.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | eigen 44 | eigen 45 | image_geometry 46 | image_geometry 47 | roscpp 48 | roscpp 49 | jir_refractive_image_geometry_msgs 50 | jir_refractive_image_geometry_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jir_refractive_image_geometry_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | message_generation 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | find_package(eigen REQUIRED) 18 | 19 | 20 | ## Generate messages in the 'msg' folder 21 | add_message_files( 22 | FILES 23 | PlanarRefractionInfo.msg 24 | ) 25 | 26 | 27 | ## Generate added messages and services with any dependencies listed here 28 | generate_messages( 29 | DEPENDENCIES 30 | std_msgs 31 | ) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS include 35 | CATKIN_DEPENDS message_runtime 36 | DEPENDS eigen 37 | ) 38 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry_msgs/include/jir_refractive_image_geometry_msgs/helpers.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JIR_REFRACTIVE_IMAGE_GEOMETRY_MSGS__HELPERS_HPP 2 | #define JIR_REFRACTIVE_IMAGE_GEOMETRY_MSGS__HELPERS_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace jir_refractive_image_geometry_msgs { 9 | 10 | inline void transform(const PlanarRefractionInfo& in, const Eigen::Affine3d& tr_in_out, PlanarRefractionInfo& out) { 11 | out = in; 12 | 13 | Eigen::Map< const Eigen::Vector3d > normal_in ( &in.normal[0] ); 14 | Eigen::Map< Eigen::Vector3d > normal_out ( &out.normal[0] ); 15 | 16 | normal_out = tr_in_out.rotation() * normal_in; 17 | 18 | out.d_0 = normal_out.dot( tr_in_out * ( in.d_0 * normal_in )); 19 | } 20 | 21 | } 22 | 23 | #endif // JIR_REFRACTIVE_IMAGE_GEOMETRY_MSGS__HELPERS_HPP 24 | -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry_msgs/msg/PlanarRefractionInfo.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64[3] normal # normal pointing towards camera 3 | float64 d_0 # distance to first plane of refraction from air to glass 4 | float64 d_1 # distance to second plane of refraction from glass to water 5 | float64 n_glass # index of refraction of glass 6 | float64 n_water # index of refraction of water -------------------------------------------------------------------------------- /C++_ROS_hydro/src/jir_refractive_image_geometry_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jir_refractive_image_geometry_msgs 4 | 0.0.1 5 | The jir_refractive_image_geometry_msgs package 6 | 7 | 8 | 9 | 10 | Jacobs Robotics 11 | 12 | 13 | 14 | 15 | CC BY-NC-ND 4.0 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | std_msgs 43 | std_msgs 44 | message_generation 45 | message_runtime 46 | eigen 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /C++_ROS_hydro/testBag.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomluc/Pinax-camera-model/e57b11918ada9c674c83786d54a61a3e86fd5419/C++_ROS_hydro/testBag.bag -------------------------------------------------------------------------------- /MATLAB/Find_correction_map/FindMap.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2017 Jacobs University Robotics Group 3 | % All rights reserved. 4 | % 5 | % 6 | % Unless specified otherwise this code examples are released under 7 | % Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | % Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | % 10 | % 11 | % If you are interested in using this code commercially, 12 | % please contact us. 13 | % 14 | % THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | % EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | % DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | % 25 | % Contact: robotics@jacobs-university.de 26 | % 27 | 28 | clear; 29 | clc; 30 | %image info 31 | width=1280; 32 | height=960; 33 | 34 | %example camera and lens parameters: 35 | K=[1403.882943803675700, 0, 616.238499749930610; 36 | 0, 1407.482157830063400, 479.792602354303430; 37 | 0, 0, 1]; 38 | 39 | distCoeffs=[-0.273139587484678, 0.216093918966729, -0.000214253361360, -0.001770247078481, 0]; 40 | 41 | 42 | %setup parameters: 43 | d1=10; %glass thickness 44 | d0=1.4282; %physical d0 distance 45 | d0virtual=[0;0;0.5851]; %virtual d0 distance 46 | d2=d0+d1; 47 | 48 | ng=1.5; %glass refraction index 49 | nw=1.335; %water refraction index 50 | mu_v=[ng;nw] ; 51 | n=[0;0;1]; 52 | 53 | %image points 54 | ImgPts=zeros(3,width*height); 55 | N=size(ImgPts,2); 56 | for i=1:width 57 | for j=1:height 58 | ImgPts(1,(j-1)*width+i)=i; 59 | ImgPts(2,(j-1)*width+i)=j; 60 | ImgPts(3,(j-1)*width+i)=1; 61 | end 62 | end 63 | 64 | Rays=inv(K)*ImgPts; 65 | M=zeros(3,N); 66 | 67 | for i=1:N 68 | (i/N)*100 69 | p=5000*Rays(:,i)+d0virtual; 70 | M(:,i) = SolveForwardProjectionCase3(d0,d2,-n,mu_v,p); 71 | 72 | end 73 | 74 | rvec=[0;0;0]; 75 | tvec=[0;0;0]; 76 | 77 | src2=zeros(1,N,3); 78 | for itr=1:N 79 | src2(1,itr,1)=M(1,itr); 80 | src2(1,itr,2)=M(2,itr); 81 | src2(1,itr,3)=M(3,itr); 82 | end 83 | 84 | imagePoints = cv.projectPoints(src2, rvec, tvec, K, distCoeffs); 85 | 86 | Mx=zeros(1,N); 87 | My=zeros(1,N); 88 | 89 | for i=1:N 90 | Mx(1,i)=imagePoints(i,1,1); 91 | My(1,i)=imagePoints(i,1,2); 92 | end 93 | 94 | save('XB3MapCenterX.txt','Mx','-ascii') 95 | save('XB3MapCenterY.txt','My','-ascii') 96 | %% 97 | mapx=zeros(height,width); 98 | mapy=zeros(height,width); 99 | 100 | for i=1:width 101 | for j=1:height 102 | mapx(j,i)=Mx((j-1)*width+i); 103 | mapy(j,i)=My((j-1)*width+i); 104 | end 105 | end 106 | 107 | img=cv.imread('testImg.jpg'); 108 | correctedImg=cv.remap(img,mapx,mapy); 109 | cv.imwrite('remapped.jpg',correctedImg); 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /MATLAB/Find_correction_map/RayTrace.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2017 Jacobs University Robotics Group 3 | % All rights reserved. 4 | % 5 | % 6 | % Unless specified otherwise this code examples are released under 7 | % Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | % Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | % 10 | % 11 | % If you are interested in using this code commercially, 12 | % please contact us. 13 | % 14 | % THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | % EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | % DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | % 25 | % Contact: robotics@jacobs-university.de 26 | % 27 | 28 | function [refPts]=RayTrace (ray0, normal, mu, mu2, d0, d1,zero) 29 | 30 | v0=ray0/norm(ray0); 31 | normal=normal/norm(normal); 32 | pi=zero+ d0*v0/(v0'*normal); 33 | 34 | normal=-normal; 35 | c=-normal'*v0; 36 | rglass=1/mu; 37 | rwater=1/mu2; 38 | v1=rglass*v0+(rglass*c -sqrt(1-rglass^2*(1-c^2)))*normal; 39 | v2=rwater*v0+(rwater*c -sqrt(1-rwater^2*(1-c^2)))*normal; 40 | normal=-normal; 41 | po=pi+ d1*v1/(v1'*normal); 42 | v1=v1/norm(v1); 43 | v2=v2/norm(v2); 44 | refPts=[v0;pi;v1;po;v2]; 45 | 46 | 47 | end -------------------------------------------------------------------------------- /MATLAB/Find_correction_map/RefractedRay.m: -------------------------------------------------------------------------------- 1 | 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Copyright (c) MERL 2012 4 | % CVPR 2012 Paper Title: A Theory of Multi-Layer Flat Refractive Geometry 5 | % Author: Amit Agrawal 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | 8 | 9 | % Compute refracted ray direction at a refraction boundary 10 | 11 | 12 | function [vr,a,b,tir] = RefractedRay(vi,n,mu1,mu2) 13 | 14 | tir = 0; 15 | 16 | 17 | n = n/norm(n); 18 | 19 | kk = mu1^2*(vi'*n)^2 - (mu1^2-mu2^2)*(vi'*vi); 20 | 21 | if(kk < 0) 22 | % total internal reflection 23 | tir = 1; 24 | a = 0; 25 | b = 0; 26 | vr = zeros(3,1); 27 | return 28 | end 29 | 30 | a = mu1/mu2; 31 | b = -mu1*(vi'*n) - sqrt(kk); 32 | b = b/mu2; 33 | 34 | vr = a*vi + b*n; 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /MATLAB/Find_correction_map/SolveForwardProjectionCase3.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % Copyright 2009 Mitsubishi Electric Research Laboratories All Rights Reserved. 4 | % 5 | % Permission to use, copy and modify this software and its documentation without fee for educational, research and non-profit purposes, is hereby granted, provided that the above copyright notice and the following three paragraphs appear in all copies. 6 | % 7 | % To request permission to incorporate this software into commercial products contact: Vice President of Marketing and Business Development; Mitsubishi Electric Research Laboratories (MERL), 201 Broadway, Cambridge, MA 02139 or . 8 | % 9 | % IN NO EVENT SHALL MERL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF MERL HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 10 | % 11 | % MERL SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND MERL HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS. 12 | % 13 | 14 | 15 | % Solve Forward projection equation for Case 3 (two layer) (Air-Medium1-Medium2) 16 | % d is the distance of medium1 from camera 17 | % d2 is the total distance of medium2 from camera 18 | 19 | % p is given 3D point 20 | % n is the normal 21 | % mu is refractive index vector 22 | 23 | % M is the 3D point on the layer closest to camera where the first 24 | % refraction happens 25 | 26 | 27 | 28 | function [M] = SolveForwardProjectionCase3(d,d2,n,mu,p) 29 | 30 | 31 | mu1 = mu(1,1); 32 | mu2 = mu(2,1); 33 | 34 | M = [0;0;1]; 35 | 36 | 37 | %find POR the plane of refraction 38 | POR = cross(n,p); 39 | POR = POR/norm(POR); 40 | 41 | % [z1,z2] defines a coordinate system on POR 42 | % axis is away from the camera. z1 is along the axis 43 | z1 = -n; 44 | z1 = z1/norm(z1); 45 | 46 | % find z2 47 | z2 = cross(POR,z1); 48 | z2 = z2/norm(z2); 49 | 50 | % find the projection of given 3D point on POR 51 | v = p'*z1; 52 | u = p'*z2; 53 | 54 | 55 | 56 | %solve 12thth degree equation 57 | s1 = (mu1^2 - 1)^2*(mu2^2 - 1)^2; 58 | 59 | s2 = (-4)*u*(mu1^2 - 1)^2*(mu2^2 - 1)^2; 60 | 61 | s3 = 4*u^2*(mu1^2 - 1)^2*(mu2^2 - 1)^2 + 2*(mu1^2 - 1)*(mu2^2 - 1)*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1)); 62 | 63 | s4 = - 2*(mu1^2 - 1)*(mu2^2 - 1)*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1)) - 4*u*(mu1^2 - 1)*(mu2^2 - 1)*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1)); 64 | 65 | s5 = ((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1))^2 + 2*(mu1^2 - 1)*(mu2^2 - 1)*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)) - 4*(mu1^2 - 1)*(mu2^2 - 1)*(d - d2)^2*(d2 - v)^2 + 4*u*(mu1^2 - 1)*(mu2^2 - 1)*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1)); 66 | 67 | s6 = -2*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1))*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1)) - 4*u*(mu1^2 - 1)*(mu2^2 - 1)*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)) - 4*d^4*mu1^2*mu2^2*u*(mu1^2 - 1)*(mu2^2 - 1); 68 | 69 | s7 = 2*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1))*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)) + (2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1))^2 - 4*(d - d2)^2*(d^2*mu1^2*(mu2^2 - 1) + d^2*mu2^2*(mu1^2 - 1))*(d2 - v)^2 + 10*d^4*mu1^2*mu2^2*u^2*(mu1^2 - 1)*(mu2^2 - 1); 70 | 71 | s8 = -2*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1))*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)) - 4*d^4*mu1^2*mu2^2*u*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1)) - 4*d^4*mu1^2*mu2^2*u^3*(mu1^2 - 1)*(mu2^2 - 1); 72 | 73 | s9 = (d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1))^2 + 2*d^4*mu1^2*mu2^2*u^2*((mu2^2 - 1)*(u^2*(mu1^2 - 1) + d^2*mu1^2) - (mu2^2 - 1)*(d - d2)^2 - (mu1^2 - 1)*(d2 - v)^2 + d^2*mu2^2*(mu1^2 - 1)) - 4*d^4*mu1^2*mu2^2*(d - d2)^2*(d2 - v)^2 + 4*d^4*mu1^2*mu2^2*u*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1)); 74 | 75 | s10 = - 2*d^4*mu1^2*mu2^2*u^2*(2*d^2*mu1^2*u*(mu2^2 - 1) + 2*d^2*mu2^2*u*(mu1^2 - 1)) - 4*d^4*mu1^2*mu2^2*u*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)); 76 | 77 | s11 = 4*d^8*mu1^4*mu2^4*u^2 + 2*d^4*mu1^2*mu2^2*u^2*(d^2*mu2^2*(u^2*(mu1^2 - 1) + d^2*mu1^2) - d^2*mu2^2*(d - d2)^2 - d^2*mu1^2*(d2 - v)^2 + d^2*mu1^2*u^2*(mu2^2 - 1)); 78 | 79 | s12 = (-4)*d^8*mu1^4*mu2^4*u^3; 80 | 81 | s13 = d^8*mu1^4*mu2^4*u^4; 82 | 83 | 84 | 85 | %[s1;s2;s3;s4;s5;s6;s7;s8;s9;s10;s11;s12;s13] 86 | 87 | 88 | sol = roots([s1;s2;s3;s4;s5;s6;s7;s8;s9;s10;s11;s12;s13]); 89 | 90 | idx = find(abs(imag(sol)) < 1e-6); 91 | if(isempty(idx)) 92 | disp('no solution'); 93 | return 94 | end 95 | 96 | sol1 = sol(idx); 97 | nn = size(sol1,1); 98 | 99 | 100 | Normal = [0;-1]; 101 | 102 | for ii = 1:nn 103 | 104 | x = sol1(ii,1); 105 | vi = [x;d]; 106 | 107 | v2 = RefractedRay(vi,Normal,1,mu1); 108 | q2 = vi + (d-d2)*v2/(v2'*Normal); 109 | 110 | 111 | v3 = RefractedRay(v2,Normal,mu1,mu2); 112 | 113 | vrd = [u;v] - q2; 114 | 115 | e = abs(vrd(1)*v3(2) - vrd(2)*v3(1)); 116 | 117 | if(e < 1e-4) 118 | M = x*z2 + d*z1; 119 | return 120 | end 121 | end 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /MATLAB/Find_correction_map/testImg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomluc/Pinax-camera-model/e57b11918ada9c674c83786d54a61a3e86fd5419/MATLAB/Find_correction_map/testImg.jpg -------------------------------------------------------------------------------- /MATLAB/MATLAB_README.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tomluc/Pinax-camera-model/e57b11918ada9c674c83786d54a61a3e86fd5419/MATLAB/MATLAB_README.pdf -------------------------------------------------------------------------------- /MATLAB/Optimal_d_0/Main.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2017 Jacobs University Robotics Group 3 | % All rights reserved. 4 | % 5 | % 6 | % Unless specified otherwise this code examples are released under 7 | % Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | % Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | % 10 | % 11 | % If you are interested in using this code commercially, 12 | % please contact us. 13 | % 14 | % THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | % EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | % DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | % 25 | % Contact: robotics@jacobs-university.de 26 | % 27 | 28 | clear; 29 | clc; 30 | 31 | global dmax dmin K d1 ng nw dif 32 | 33 | K=[900 0 512; 34 | 0 900 384; 35 | 0 0 1]; %camera intrinsics 36 | d1=10; %glass thickness in mm 37 | ng=1.492; %glass refraction index 38 | nw=1.345; %water refraction index 39 | 40 | 41 | x0=[0.1]; 42 | options = optimset('Display','iter','TolFun',1e-8,'TolX',1e-8,'MaxIter',100000,'MaxFunEvals',100000); 43 | x = lsqnonlin(@optim_d_0,x0,[],[],options); 44 | optimal_physical_d_0=x 45 | virtual_d_0=(dmin+dmax)/2 46 | approxErr=dif -------------------------------------------------------------------------------- /MATLAB/Optimal_d_0/RayTrace.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2017 Jacobs University Robotics Group 3 | % All rights reserved. 4 | % 5 | % 6 | % Unless specified otherwise this code examples are released under 7 | % Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | % Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | % 10 | % 11 | % If you are interested in using this code commercially, 12 | % please contact us. 13 | % 14 | % THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | % EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | % DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | % 25 | % Contact: robotics@jacobs-university.de 26 | % 27 | 28 | function [refPts]=RayTrace (ray0, normal, mu, mu2, d0, d1,zero) 29 | 30 | v0=ray0/norm(ray0); 31 | normal=normal/norm(normal); 32 | pi=zero+ d0*v0/(v0'*normal); 33 | normal=-normal; 34 | c=-normal'*v0; 35 | rglass=1/mu; 36 | rwater=1/mu2; 37 | v1=rglass*v0+(rglass*c -sqrt(1-rglass^2*(1-c^2)))*normal; 38 | v2=rwater*v0+(rwater*c -sqrt(1-rwater^2*(1-c^2)))*normal; 39 | normal=-normal; 40 | po=pi+ d1*v1/(v1'*normal); 41 | v1=v1/norm(v1); 42 | v2=v2/norm(v2); 43 | refPts=[v0;pi;v1;po;v2]; 44 | 45 | end -------------------------------------------------------------------------------- /MATLAB/Optimal_d_0/optim_d_0.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2017 Jacobs University Robotics Group 3 | % All rights reserved. 4 | % 5 | % 6 | % Unless specified otherwise this code examples are released under 7 | % Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 8 | % Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 9 | % 10 | % 11 | % If you are interested in using this code commercially, 12 | % please contact us. 13 | % 14 | % THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 15 | % EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | % DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 18 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | % 25 | % Contact: robotics@jacobs-university.de 26 | 27 | function [diff] = optim_d_0(x) 28 | 29 | global dmax dmin K d1 ng nw dif 30 | 31 | d0=x; 32 | 33 | ImgPts=zeros(3,300); 34 | 35 | for i=1:20 36 | for j=1:15 37 | ImgPts(1,(j-1)*20+i)=50*i; 38 | ImgPts(2,(j-1)*20+i)=50*j; 39 | ImgPts(3,(j-1)*20+i)=1; 40 | end 41 | end 42 | 43 | ZeroRays=inv(K)*ImgPts; 44 | 45 | normal=[0;0;1]; 46 | 47 | t=[0;0;0]; 48 | 49 | 50 | dmin=9999; 51 | dmax=-9999; 52 | 53 | for i=1:300 54 | xl= RayTrace (ZeroRays(:,i), normal, ng, nw, d0, d1,t); 55 | xl=xl(:); 56 | po=[xl(10);xl(11);xl(12)]; 57 | v2=[xl(13);xl(14);xl(15)]; 58 | pom=abs(po(3)-v2(3)*(po(1)/(2*v2(1)) + po(2)/(2*v2(2)))); 59 | dmin=min(dmin,pom); 60 | dmax=max(dmax,pom); 61 | end 62 | dif=dmax-dmin; 63 | diff=dif; 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /README_LICENSE: -------------------------------------------------------------------------------- 1 | # Pinax-camera-model 2 | When using this code in scientific work please cite: 3 | 4 | Tomasz Łuczyński, Max Pfingsthorn, Andreas Birk 5 | The Pinax-model for accurate and efficient refraction correction of underwater cameras in flat-pane housings 6 | Ocean Engineering, Volume 133, 2017, Pages 9-22, ISSN 0029-8018, http://dx.doi.org/10.1016/j.oceaneng.2017.01.029. 7 | (http://www.sciencedirect.com/science/article/pii/S0029801817300434) 8 | 9 | Abstract: The calibration and refraction correction process for underwater cameras with flat-pane interfaces is presented that is very easy and convenient to use in real world applications while yielding very accurate results. The correction is derived from an analysis of the axial camera model for underwater cameras, which is among others computationally hard to tackle. It is shown how realistic constraints on the distance of the camera to the window can be exploited, which leads to an approach dubbed Pinax Model as it combines aspects of a virtual pinhole model with the projection function from the axial camera model. It allows the pre-computation of a lookup-table for very fast refraction correction of the flat-pane with high accuracy. The model takes the refraction indices of water into account, especially with respect to salinity, and it is therefore sufficient to calibrate the underwater camera only once in air. It is demonstrated by real world experiments with several underwater cameras in different salt and sweet water conditions that the proposed process outperforms standard methods. Among others, it is shown how the presented method leads to accurate results with single in-air calibration and even with just estimated salinity values. 10 | 11 | ******************** LICENSE & DISCALIMER************************* 12 | Examples are released as Matlab and C/C++ ROS code. 13 | Unless specified otherwise in the respective files this 14 | code was developed within Jacobs Robotics Group, 15 | Jacobs University Bremen gGmbH. 16 | 17 | THIS SOFTWARE IS PROVIDED BY Jacobs Robotics ``AS IS'' AND ANY 18 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL Jacobs Robotics BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | Unless specified otherwise this code examples are released under 29 | Creative Commons CC BY-NC-ND 4.0 license (free for non-commercial use). 30 | Details may be found here: https://creativecommons.org/licenses/by-nc-nd/4.0/ 31 | 32 | If you wish to commercialize the Pinax model please contact robotics@jacobs-university.de 33 | 34 | 35 | --------------------------------------------------------------------------------