├── LICENSE.md ├── README.md ├── calibration_algorithms ├── CMakeLists.txt ├── include │ ├── calibration_algorithms │ │ ├── checkerboard_corners_extraction.h │ │ ├── checkerboard_extraction.h │ │ ├── plane_fit.h │ │ └── plane_to_plane_calibration.h │ └── impl │ │ └── calibration_algorithms │ │ └── plane_fit.hpp ├── package.xml ├── src │ └── calibration_algorithms │ │ ├── checkerboard_corners_extraction.cpp │ │ ├── checkerboard_extraction.cpp │ │ └── plane_to_plane_calibration.cpp └── test │ ├── test_calibration_algorithms.cpp │ └── test_plane_to_plane_calibration.cpp ├── calibration_common ├── CMakeLists.txt ├── include │ ├── calibration_common │ │ ├── base │ │ │ ├── base.h │ │ │ ├── definitions.h │ │ │ ├── eigen_bugfix.h │ │ │ ├── eigen_cloud.h │ │ │ ├── geometry.h │ │ │ ├── opencv_eigen_conversions.h │ │ │ └── polynomial.h │ │ ├── depth │ │ │ ├── depth.h │ │ │ ├── sensor.h │ │ │ └── view.h │ │ ├── objects │ │ │ ├── base_object.h │ │ │ ├── checkerboard.h │ │ │ ├── objects.h │ │ │ ├── planar_object.h │ │ │ ├── sensor.h │ │ │ ├── view.h │ │ │ └── with_points.h │ │ └── pinhole │ │ │ ├── camera_model.h │ │ │ ├── pinhole.h │ │ │ ├── sensor.h │ │ │ └── view.h │ └── impl │ │ └── calibration_common │ │ └── pinhole │ │ └── camera_model.hpp ├── package.xml └── test │ ├── base │ ├── test_opencv_eigen_conversions.cpp │ └── test_polynomial.cpp │ ├── objects │ └── test_checkerboard.cpp │ ├── pinhole │ └── test_camera_model.cpp │ └── test_calibration_common.cpp ├── calibration_msgs ├── CMakeLists.txt ├── action │ └── CheckerboardExtraction.action ├── include │ └── calibration_msgs │ │ └── calibration_common_conversions.h ├── msg │ ├── CalibrationPose.msg │ ├── Checkerboard.msg │ ├── CheckerboardCornersView.msg │ ├── CheckerboardDepthView.msg │ ├── Point2D.msg │ ├── Point2DArray.msg │ ├── PointArray.msg │ └── Polynomial.msg ├── package.xml ├── src │ └── calibration_msgs │ │ └── calibration_common_conversions.cpp └── srv │ ├── GetCalibrationResults.srv │ └── GetDeviceInfo.srv ├── calibration_pcl ├── CMakeLists.txt ├── include │ ├── calibration_pcl │ │ ├── base │ │ │ ├── base.h │ │ │ ├── definitions.h │ │ │ └── pcl_eigen_conversions.h │ │ └── depth │ │ │ └── view.h │ └── impl │ │ └── calibration_pcl │ │ └── utilities │ │ ├── point_plane_extraction.hpp │ │ └── point_plane_extraction_gui.hpp ├── package.xml └── test │ ├── base │ └── test_pcl_eigen_conversions.cpp │ └── test_calibration_pcl.cpp ├── calibration_ros ├── CMakeLists.txt ├── include │ └── calibration_ros │ │ ├── devices │ │ └── ros_device.h │ │ ├── sensors │ │ ├── depth_sensor.h │ │ ├── pinhole_camera.h │ │ └── ros_sensor.h │ │ └── visualization │ │ └── objects.h ├── package.xml └── src │ └── calibration_ros │ ├── devices │ └── ros_device.cpp │ ├── sensors │ ├── depth_sensor.cpp │ └── pinhole_camera.cpp │ └── visualization │ └── objects.cpp ├── calibration_toolkit ├── CMakeLists.txt ├── package.xml └── scripts │ └── install_ceres.sh └── multisensor_calibration ├── CMakeLists.txt ├── conf ├── checkerboard.yaml ├── network.yaml └── pointgrey_camera_default.yaml ├── include └── multisensor_calibration │ ├── calibration.h │ ├── common.h │ ├── corner_based │ ├── errors.h │ └── plane_errors.h │ ├── device_node.h │ ├── master_node.h │ └── optimization.h ├── launch ├── camera_node.launch ├── kinect1_node.launch ├── kinect2.launch ├── kinect2_bridge.launch ├── kinect2_rgb.launch ├── master_node.launch ├── pg_camera.launch └── pg_camera_old.launch ├── package.xml └── src └── multisensor_calibration ├── calibration.cpp ├── device_node.cpp ├── master_node.cpp └── optimization.cpp /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013-2014, Filippo Basso \ 2 | 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 3. Neither the name of the copyright holder(s) nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Calibration Toolkit 2 | 3 | The package was tested with **Ubuntu 14.04** and **ROS Indigo**. 4 | 5 | ### Dependencies 6 | 7 | The package needs **Eigen 3.2+** to compile (not available in Ubuntu 12.04 repositories). 8 | 9 | ### License 10 | 11 | ``` 12 | Copyright (c) 2013-, Filippo Basso 13 | All rights reserved. 14 | 15 | Redistribution and use in source and binary forms, with or without 16 | modification, are permitted provided that the following conditions are met: 17 | 1. Redistributions of source code must retain the above copyright 18 | notice, this list of conditions and the following disclaimer. 19 | 2. Redistributions in binary form must reproduce the above copyright 20 | notice, this list of conditions and the following disclaimer in the 21 | documentation and/or other materials provided with the distribution. 22 | 3. Neither the name of the copyright holder(s) nor the 23 | names of its contributors may be used to endorse or promote products 24 | derived from this software without specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 27 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 28 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 29 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 30 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 31 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 32 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 33 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 34 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 35 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 36 | ``` 37 | -------------------------------------------------------------------------------- /calibration_algorithms/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibration_algorithms) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | calibration_common 11 | calibration_pcl 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################### 24 | ## catkin specific configuration ## 25 | ################################### 26 | ## The catkin_package macro generates cmake config files for your package 27 | ## Declare things to be passed to dependent projects 28 | ## INCLUDE_DIRS: uncomment this if you package contains header files 29 | ## LIBRARIES: libraries you create in this project that dependent projects also need 30 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 31 | ## DEPENDS: system dependencies of this project that dependent projects also need 32 | catkin_package( 33 | INCLUDE_DIRS 34 | include 35 | LIBRARIES 36 | ${PROJECT_NAME} 37 | plane_to_plane_calibration 38 | CATKIN_DEPENDS 39 | calibration_common 40 | calibration_pcl 41 | # DEPENDS 42 | ) 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | add_custom_target(${PROJECT_NAME}_HEADERS 49 | SOURCES 50 | include/${PROJECT_NAME}/plane_fit.h 51 | include/impl/${PROJECT_NAME}/plane_fit.hpp 52 | 53 | include/${PROJECT_NAME}/plane_to_plane_calibration.h 54 | 55 | include/${PROJECT_NAME}/checkerboard_corners_extraction.h 56 | include/${PROJECT_NAME}/checkerboard_extraction.h 57 | ) 58 | 59 | ## Specify additional locations of header files 60 | ## Your package locations should be listed before other locations 61 | include_directories( 62 | include 63 | ${catkin_INCLUDE_DIRS} 64 | ) 65 | 66 | ## Declare a cpp library 67 | #add_library(calibration_algorithms 68 | # src/${PROJECT_NAME}/calibration_algorithms.cpp 69 | #) 70 | add_library(${PROJECT_NAME} 71 | src/${PROJECT_NAME}/checkerboard_corners_extraction.cpp 72 | src/${PROJECT_NAME}/checkerboard_extraction.cpp 73 | ) 74 | 75 | add_library(plane_to_plane_calibration 76 | src/${PROJECT_NAME}/plane_to_plane_calibration.cpp 77 | ) 78 | 79 | ## Declare a cpp executable 80 | 81 | ## Add cmake target dependencies of the executable/library 82 | ## as an example, message headers may need to be generated before nodes 83 | # add_dependencies(calibration_algorithms_node calibration_algorithms_generate_messages_cpp) 84 | 85 | ## Specify libraries to link a library or executable target against 86 | 87 | target_link_libraries(${PROJECT_NAME} 88 | ${catkin_LIBRARIES} 89 | ) 90 | 91 | ############# 92 | ## Install ## 93 | ############# 94 | 95 | # all install targets should use catkin DESTINATION variables 96 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 97 | 98 | ## Mark executable scripts (Python etc.) for installation 99 | ## in contrast to setup.py, you can choose the destination 100 | # install(PROGRAMS 101 | # scripts/my_python_script 102 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 103 | # ) 104 | 105 | ## Mark executables and/or libraries for installation 106 | # install(TARGETS calibration_algorithms calibration_algorithms_node 107 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 108 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 109 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 110 | # ) 111 | 112 | ## Mark cpp header files for installation 113 | # install(DIRECTORY include/${PROJECT_NAME}/ 114 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 115 | # FILES_MATCHING PATTERN "*.h" 116 | # PATTERN ".svn" EXCLUDE 117 | # ) 118 | 119 | ## Mark other files for installation (e.g. launch and bag files, etc.) 120 | # install(FILES 121 | # # myfile1 122 | # # myfile2 123 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 124 | # ) 125 | 126 | ############# 127 | ## Testing ## 128 | ############# 129 | 130 | ## Add gtest based cpp test target and link libraries 131 | catkin_add_gtest(${PROJECT_NAME}-test 132 | test/test_calibration_algorithms.cpp 133 | test/test_plane_to_plane_calibration.cpp 134 | ) 135 | 136 | if(TARGET ${PROJECT_NAME}-test) 137 | target_link_libraries(${PROJECT_NAME}-test 138 | ${PROJECT_NAME} 139 | plane_to_plane_calibration 140 | ) 141 | endif() 142 | 143 | ## Add folders to be run by python nosetests 144 | # catkin_add_nosetests(test) 145 | -------------------------------------------------------------------------------- /calibration_algorithms/include/calibration_algorithms/checkerboard_corners_extraction.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_CORNERS_EXTRACTION_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_CORNERS_EXTRACTION_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | class Checkerboard; 41 | 42 | /** 43 | * @brief Automatically extract checkerboard corners from an image. 44 | */ 45 | class CheckerboardCornersExtraction 46 | { 47 | public: 48 | 49 | struct ExtractedCorners 50 | { 51 | bool pattern_found; 52 | Cloud2 corners; 53 | }; 54 | 55 | void 56 | setImage(const cv::Mat & image); 57 | 58 | // bool 59 | // perform(const Checkerboard & checkerboard, 60 | // std::vector & corners) const; 61 | 62 | ExtractedCorners 63 | perform(const Checkerboard & checkerboard) const; 64 | 65 | protected: 66 | 67 | cv::Mat image_; ///< The image. 68 | cv::Mat gray_; ///< The grayscale image. 69 | 70 | }; 71 | 72 | 73 | } // namespace calib 74 | } // namespace unipd 75 | #endif // UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_CORNERS_EXTRACTION_H_ 76 | -------------------------------------------------------------------------------- /calibration_algorithms/include/calibration_algorithms/checkerboard_extraction.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_EXTRACTION_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_EXTRACTION_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace unipd 37 | { 38 | namespace calib 39 | { 40 | class PlanarObject; 41 | class Checkerboard; 42 | class CheckerboardCorners; 43 | class PinholeSensor; 44 | class PinholeDepthSensor; 45 | 46 | template 47 | class PinholePointsView; 48 | 49 | template 50 | class DepthViewPCL; 51 | 52 | class CheckerboardExtraction 53 | { 54 | public: 55 | 56 | struct ImageResult 57 | { 58 | bool extracted; 59 | std::shared_ptr> view; 60 | }; 61 | 62 | struct DepthResult 63 | { 64 | bool extracted; 65 | std::shared_ptr> view; 66 | }; 67 | 68 | CheckerboardExtraction () 69 | : color_to_depth_transform_(Transform3::Identity()) 70 | { 71 | // Do nothing 72 | } 73 | 74 | void 75 | setExtractionNumber (int number); 76 | 77 | void 78 | setImage (const cv::Mat & image); 79 | 80 | void 81 | setCloud (const pcl::PointCloud::ConstPtr & cloud); 82 | 83 | void 84 | setColorSensor (const std::shared_ptr & sensor); 85 | 86 | void 87 | setDepthSensor (const std::shared_ptr & sensor); 88 | 89 | void 90 | setCheckerboard (const std::shared_ptr & checkerboard); 91 | 92 | void 93 | setColorToDepthTransform (const Transform3 & transform); 94 | 95 | ImageResult 96 | performImage () const; 97 | 98 | DepthResult 99 | performDepth (const ImageResult & image_result) const; 100 | 101 | private: 102 | 103 | int number_ = 0; 104 | 105 | cv::Mat image_; 106 | pcl::PointCloud::ConstPtr cloud_; 107 | 108 | std::shared_ptr color_sensor_; 109 | std::shared_ptr depth_sensor_; 110 | Transform3 color_to_depth_transform_; 111 | 112 | std::shared_ptr checkerboard_; 113 | 114 | }; 115 | 116 | } // namespace calib 117 | } // namespace unipd 118 | 119 | #endif // UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_CHECKERBOARD_EXTRACTION_H_ 120 | -------------------------------------------------------------------------------- /calibration_algorithms/include/calibration_algorithms/plane_fit.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_FIT_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_FIT_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | template 40 | Plane3_ 41 | plane_fit(const Cloud3_ & points); 42 | 43 | } // namespace calib 44 | } // namespace unipd 45 | 46 | #include 47 | 48 | #endif // UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_FIT_H_ 49 | -------------------------------------------------------------------------------- /calibration_algorithms/include/calibration_algorithms/plane_to_plane_calibration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_TO_PLANE_CALIBRATION_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_TO_PLANE_CALIBRATION_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | /** 40 | * @brief The PlaneToPlaneCalibration class 41 | * C++ implementation of: 42 | * R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration of a laser rangefinder to a camera,” Tech. Rep., 2005. 43 | */ 44 | class PlaneToPlaneCalibration 45 | { 46 | public: 47 | 48 | void 49 | addPlanePair (const std::pair & pair) 50 | { 51 | plane_pair_vec_.push_back(pair); 52 | } 53 | 54 | bool 55 | canEstimateTransform () const 56 | { 57 | return plane_pair_vec_.size() >= 10; 58 | } 59 | 60 | Transform3 61 | estimateTransform () const 62 | { 63 | return estimateTransform(plane_pair_vec_); 64 | } 65 | 66 | static Transform3 67 | estimateTransform (const std::vector> & plane_pair_vec); 68 | 69 | protected: 70 | 71 | std::vector> plane_pair_vec_; 72 | 73 | }; 74 | 75 | } // namespace calib 76 | } // namespace unipd 77 | #endif // UNIPD_CALIBRATION_CALIBRATION_ALGORITHMS_PLANE_TO_PLANE_CALIBRATION_H_ 78 | -------------------------------------------------------------------------------- /calibration_algorithms/include/impl/calibration_algorithms/plane_fit.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_ 30 | #define UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | template 41 | Plane3_ 42 | plane_fit (const Cloud3_ & points) 43 | { 44 | assert(points.elements() > 1); 45 | 46 | Point3_ centroid = Point3_::Zero(); 47 | typename Cloud3_::Container diff(3, points.elements()); 48 | Size1 count = 0; 49 | for (Size1 i = 0; i < points.elements(); ++i) 50 | { 51 | if (points[i].allFinite()) 52 | { 53 | centroid += points[i]; 54 | diff.col(count++) = points[i]; 55 | } 56 | } 57 | centroid /= count; 58 | diff.conservativeResize(3, count); 59 | diff.colwise() -= centroid; 60 | 61 | Eigen::Matrix covariance_matrix = diff * diff.transpose() / ScalarT_(count - 1); 62 | Eigen::SelfAdjointEigenSolver > solver(covariance_matrix, Eigen::ComputeEigenvectors); 63 | 64 | Point3_ eigen_vector = solver.eigenvectors().col(0); 65 | return Plane3_(eigen_vector, -eigen_vector.dot(centroid)); 66 | } 67 | 68 | } // namespace calib 69 | } // namespace unipd 70 | #endif // UNIPD_CALIBRATION_IMPL_CALIBRATION_ALGORITHMS_CERES_PLANE_FIT_HPP_ 71 | -------------------------------------------------------------------------------- /calibration_algorithms/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | calibration_algorithms 5 | 1.0.0 6 | 7 | The calibration_algorithms package 8 | 9 | Filippo Basso 10 | BSD 11 | 12 | 13 | 14 | catkin 15 | 16 | calibration_common 17 | calibration_pcl 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /calibration_algorithms/src/calibration_algorithms/checkerboard_corners_extraction.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | namespace unipd 40 | { 41 | namespace calib 42 | { 43 | 44 | void 45 | CheckerboardCornersExtraction::setImage (const cv::Mat & image) 46 | { 47 | image_ = image; 48 | if (image_.channels() > 1) 49 | cv::cvtColor(image_, gray_, CV_BGR2GRAY); 50 | else if (image_.depth() == CV_16U) 51 | { 52 | cv::Mat tmp; 53 | double min, max; 54 | cv::minMaxLoc(image_, &min, &max); 55 | image_.convertTo(tmp, CV_8U, 255.0 / max); 56 | cv::equalizeHist(tmp, gray_); 57 | } 58 | else 59 | gray_ = image_; 60 | } 61 | 62 | auto 63 | CheckerboardCornersExtraction::perform (const Checkerboard & checkerboard) const -> ExtractedCorners 64 | { 65 | std::vector cv_corners; 66 | cv::Size pattern_size(checkerboard.cols(), checkerboard.rows()); 67 | 68 | if (cv::findChessboardCorners(gray_, pattern_size, cv_corners, cv::CALIB_CB_FAST_CHECK)) 69 | { 70 | cv::cornerSubPix(gray_, cv_corners, cv::Size(2, 2), cv::Size(-1, -1), 71 | cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01)); 72 | return ExtractedCorners{true, Cloud2(opencv2eigen(cv_corners), checkerboard.corners().size())}; 73 | } 74 | else 75 | { 76 | return ExtractedCorners{false, Cloud2(Size2{0, 0})}; 77 | } 78 | 79 | } 80 | 81 | } // namespace calib 82 | } // namespace unipd 83 | -------------------------------------------------------------------------------- /calibration_algorithms/src/calibration_algorithms/plane_to_plane_calibration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | namespace unipd 33 | { 34 | namespace calib 35 | { 36 | 37 | Transform3 38 | PlaneToPlaneCalibration::estimateTransform (const std::vector> & plane_pair_vector) 39 | { 40 | const Size1 SIZE = plane_pair_vector.size(); 41 | assert(SIZE >= 10); 42 | 43 | Eigen::Matrix normals_1(SIZE, 3); 44 | Eigen::Matrix normals_2(SIZE, 3); 45 | Eigen::Matrix distances_1(SIZE); 46 | Eigen::Matrix distances_2(SIZE); 47 | 48 | for (int i = 0; i < SIZE; ++i) 49 | { 50 | const Plane3 & plane_1 = plane_pair_vector[i].first; 51 | const Plane3 & plane_2 = plane_pair_vector[i].second; 52 | 53 | if (plane_1.offset() > 0) 54 | { 55 | normals_1.row(i) = -plane_1.normal(); 56 | distances_1(i) = plane_1.offset(); 57 | } 58 | else 59 | { 60 | normals_1.row(i) = plane_1.normal(); 61 | distances_1(i) = -plane_1.offset(); 62 | } 63 | 64 | if (plane_2.offset() > 0) 65 | { 66 | normals_2.row(i) = -plane_2.normal(); 67 | distances_2(i) = plane_2.offset(); 68 | } 69 | else 70 | { 71 | normals_2.row(i) = plane_2.normal(); 72 | distances_2(i) = -plane_2.offset(); 73 | } 74 | } 75 | 76 | Transform3 transform; 77 | 78 | Eigen::Matrix USV = normals_2.transpose() * normals_1; 79 | Eigen::JacobiSVD > svd; 80 | svd.compute(USV, Eigen::ComputeFullU | Eigen::ComputeFullV); 81 | transform.linear() = svd.matrixV() * svd.matrixU().transpose(); 82 | 83 | // transform.translation() = (normals_1.transpose() * normals_1).inverse() * normals_1.transpose() * (distances_1 - distances_2); 84 | 85 | // Use QR decomposition 86 | Eigen::ColPivHouseholderQR > qr(normals_1.rows(), 3); 87 | qr.compute(normals_1); 88 | Eigen::Matrix R = qr.matrixR().template triangularView(); 89 | transform.translation() = (qr.colsPermutation() * R.transpose() * R * qr.colsPermutation().transpose()).inverse() * normals_1.transpose() * (distances_1 - distances_2); 90 | 91 | return transform; 92 | } 93 | 94 | } // namespace calib 95 | } // namespace unipd 96 | -------------------------------------------------------------------------------- /calibration_algorithms/test/test_calibration_algorithms.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | 31 | int 32 | main(int argc, char ** argv) 33 | { 34 | ::testing::InitGoogleTest(&argc, argv); 35 | return RUN_ALL_TESTS(); 36 | } 37 | -------------------------------------------------------------------------------- /calibration_algorithms/test/test_plane_to_plane_calibration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | using namespace unipd::calib; 33 | 34 | TEST(PlaneToPlaneCalibration, estimateTransform) 35 | { 36 | PlaneToPlaneCalibration calib; 37 | calib.addPlanePair(std::make_pair(Plane3(Vector3(0.112691, -0.276207, 0.954469), -1.35423), 38 | Plane3(Vector3(0.218832, -0.330751, 0.917996), -3.0117))); 39 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.0162984, -0.123521, 0.992208), -1.33999), 40 | Plane3(Vector3(0.0756903, -0.127099, 0.988998), -3.21313))); 41 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.153918, -0.0842592, 0.984484), -1.26553), 42 | Plane3(Vector3(-0.0650428, -0.121503, 0.990458), -3.08435))); 43 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.413197, -0.118166, 0.902942), -1.17928), 44 | Plane3(Vector3(-0.325596, -0.15166, 0.933266), -2.79794))); 45 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.093132, -0.0931221, 0.991289), -1.2432), 46 | Plane3(Vector3(0.00197333, -0.125383, 0.992106), -3.10749))); 47 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.139159, -0.117658, 0.983256), -1.27124), 48 | Plane3(Vector3(-0.0612503, -0.150665, 0.986686), -3.08316))); 49 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.275229, -0.122331, 0.953564), -1.27538), 50 | Plane3(Vector3(-0.188487, -0.141886, 0.971772), -3.0207))); 51 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.0924412, -0.168909, 0.981287), -0.670834), 52 | Plane3(Vector3(-0.00893065, -0.195327, 0.980697), -2.46828))); 53 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.248585, -0.0603065, 0.966731), -0.730401), 54 | Plane3(Vector3(-0.157237, -0.0922465, 0.983243), -2.52748))); 55 | calib.addPlanePair(std::make_pair(Plane3(Vector3(-0.60247, -0.095165, 0.792448), -0.732968), 56 | Plane3(Vector3(-0.507328, -0.114602, 0.854099), -2.19752))); 57 | 58 | Transform3 transform = calib.estimateTransform(); 59 | EXPECT_TRUE(transform.translation().isApprox(Vector3(-0.0547, -0.6576, -1.9342), 0.001)); 60 | 61 | } 62 | -------------------------------------------------------------------------------- /calibration_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibration_common) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | cmake_modules 11 | image_geometry 12 | ) 13 | 14 | find_package(Eigen REQUIRED) 15 | 16 | ################################### 17 | ## catkin specific configuration ## 18 | ################################### 19 | ## The catkin_package macro generates cmake config files for your package 20 | ## Declare things to be passed to dependent projects 21 | ## INCLUDE_DIRS: uncomment this if you package contains header files 22 | ## LIBRARIES: libraries you create in this project that dependent projects also need 23 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 24 | ## DEPENDS: system dependencies of this project that dependent projects also need 25 | catkin_package( 26 | INCLUDE_DIRS include ${EIGEN_INCLUDE_DIRS} 27 | # LIBRARIES ${PROJECT_NAME} 28 | CATKIN_DEPENDS 29 | cmake_modules 30 | image_geometry 31 | # DEPENDS 32 | # Eigen 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | add_custom_target(${PROJECT_NAME}_HEADERS 40 | SOURCES 41 | include/${PROJECT_NAME}/base/base.h 42 | include/${PROJECT_NAME}/base/definitions.h 43 | include/${PROJECT_NAME}/base/geometry.h 44 | include/${PROJECT_NAME}/base/eigen_cloud.h 45 | include/${PROJECT_NAME}/base/opencv_eigen_conversions.h 46 | include/${PROJECT_NAME}/base/polynomial.h 47 | include/${PROJECT_NAME}/base/eigen_bugfix.h 48 | 49 | include/${PROJECT_NAME}/depth/depth.h 50 | include/${PROJECT_NAME}/depth/sensor.h 51 | include/${PROJECT_NAME}/depth/view.h 52 | 53 | include/${PROJECT_NAME}/objects/base_object.h 54 | include/${PROJECT_NAME}/objects/checkerboard.h 55 | include/${PROJECT_NAME}/objects/objects.h 56 | include/${PROJECT_NAME}/objects/planar_object.h 57 | include/${PROJECT_NAME}/objects/sensor.h 58 | include/${PROJECT_NAME}/objects/view.h 59 | include/${PROJECT_NAME}/objects/with_points.h 60 | 61 | include/${PROJECT_NAME}/pinhole/camera_model.h 62 | include/${PROJECT_NAME}/pinhole/pinhole.h 63 | include/${PROJECT_NAME}/pinhole/sensor.h 64 | include/${PROJECT_NAME}/pinhole/view.h 65 | include/impl/${PROJECT_NAME}/pinhole/camera_model.hpp 66 | ) 67 | 68 | include_directories( 69 | include 70 | ${catkin_INCLUDE_DIRS} 71 | ${EIGEN_INCLUDE_DIRS} 72 | ) 73 | 74 | # add_dependencies(calibration_common_node calibration_common_generate_messages_cpp) 75 | 76 | ############# 77 | ## Install ## 78 | ############# 79 | 80 | # all install targets should use catkin DESTINATION variables 81 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 82 | 83 | ## Mark executable scripts (Python etc.) for installation 84 | ## in contrast to setup.py, you can choose the destination 85 | # install(PROGRAMS 86 | # scripts/my_python_script 87 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 88 | # ) 89 | 90 | ## Mark executables and/or libraries for installation 91 | # install(TARGETS calibration_common calibration_common_node 92 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | # ) 96 | 97 | ## Mark cpp header files for installation 98 | # install(DIRECTORY include/${PROJECT_NAME}/ 99 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 100 | # FILES_MATCHING PATTERN "*.h" 101 | # PATTERN ".svn" EXCLUDE 102 | # ) 103 | 104 | ## Mark other files for installation (e.g. launch and bag files, etc.) 105 | # install(FILES 106 | # # myfile1 107 | # # myfile2 108 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 109 | # ) 110 | 111 | ############# 112 | ## Testing ## 113 | ############# 114 | 115 | catkin_add_gtest(${PROJECT_NAME}-test 116 | #add_executable(${PROJECT_NAME}-test 117 | test/test_${PROJECT_NAME}.cpp 118 | test/base/test_polynomial.cpp 119 | test/base/test_opencv_eigen_conversions.cpp 120 | test/objects/test_checkerboard.cpp 121 | test/pinhole/test_camera_model.cpp 122 | ) 123 | 124 | if(TARGET ${PROJECT_NAME}-test) 125 | target_link_libraries(${PROJECT_NAME}-test 126 | ${catkin_LIBRARIES} 127 | ) 128 | endif() 129 | 130 | ## Add folders to be run by python nosetests 131 | # catkin_add_nosetests(test) 132 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/base/base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_BASE_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_BASE_H_ 31 | 32 | #include "definitions.h" 33 | #include "eigen_cloud.h" 34 | #include "geometry.h" 35 | #include "polynomial.h" 36 | 37 | #endif /* UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_BASE_H_ */ 38 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/base/definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_DEFINITIONS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_DEFINITIONS_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace unipd 37 | { 38 | namespace calib 39 | { 40 | 41 | using Scalar = double; 42 | 43 | template 44 | struct Dimension {}; 45 | 46 | using Size1 = size_t; 47 | 48 | struct Size2 49 | { 50 | Size1 x; 51 | Size1 y; 52 | }; 53 | 54 | template <> 55 | struct Dimension 56 | { 57 | static const int value = 1; 58 | }; 59 | 60 | template <> 61 | struct Dimension 62 | { 63 | static const int value = 2; 64 | }; 65 | 66 | inline bool 67 | operator == (const Size2 & lhs, const Size2 & rhs) 68 | { 69 | return (lhs.x == rhs.x) and (lhs.y == rhs.y); 70 | } 71 | 72 | inline Size1 73 | reduce (const Size2 & size) 74 | { 75 | return size.x * size.y; 76 | } 77 | 78 | using Index1 = int; 79 | struct Index2 80 | { 81 | Index1 x; 82 | Index1 y; 83 | }; 84 | 85 | inline bool 86 | operator == (const Index2 & lhs, const Index2 & rhs) 87 | { 88 | return (lhs.x == rhs.x) and (lhs.y == rhs.y); 89 | } 90 | 91 | using Indices1 = std::vector; 92 | using Indices2 = std::vector; 93 | 94 | template <> 95 | struct Dimension 96 | { 97 | static const int value = 1; 98 | }; 99 | template <> 100 | struct Dimension 101 | { 102 | static const int value = 2; 103 | }; 104 | 105 | template 106 | boost::shared_ptr std2boost (std::shared_ptr & ptr) 107 | { 108 | return boost::shared_ptr(ptr.get(), [ptr](T*) mutable {ptr.reset();}); 109 | } 110 | 111 | template 112 | std::shared_ptr boost2std (boost::shared_ptr & ptr) 113 | { 114 | return std::shared_ptr(ptr.get(), [ptr](T*) mutable {ptr.reset();}); 115 | } 116 | 117 | } // namespace calib 118 | } // namespace unipd 119 | 120 | #endif /* UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_DEFINITIONS_H_ */ 121 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/base/eigen_bugfix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Manuel Yguel 3 | * Copyright (c) 2014, Filippo Basso 4 | * 5 | * This Source Code Form is subject to the terms of the Mozilla 6 | * Public License v. 2.0. If a copy of the MPL was not distributed 7 | * with this file, You can obtain one at http://mozilla.org/MPL/2.0 8 | */ 9 | 10 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_EIGEN_BUGFIX_H_ 11 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_EIGEN_BUGFIX_H_ 12 | 13 | #include 14 | 15 | namespace Eigen 16 | { 17 | 18 | /** 19 | * @brief Bugfix for Eigen::poly_eval method 20 | * @returns The evaluation of the polynomial at x using stabilized Horner algorithm. 21 | * 22 | * @param[in] polynomial The vector of coefficients of the polynomial ordered 23 | * by degrees i.e. polynomial[i] is the coefficient of degree i of the polynomial 24 | * e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [1, 0, 3] \f$. 25 | * @param[in] x The value to evaluate the polynomial at. 26 | */ 27 | template 28 | typename DenseBase::Scalar poly_eval_bugfix(const DenseBase & polynomial, 29 | const typename DenseBase::Scalar & x) 30 | { 31 | typedef typename Eigen::DenseBase::Scalar Scalar; 32 | typedef typename Eigen::NumTraits::Real Real; 33 | 34 | if (numext::abs2(x) <= Real(1)) 35 | return poly_eval_horner(polynomial, x); 36 | else 37 | { 38 | Scalar val = polynomial[0]; 39 | Scalar inv_x = Scalar(1.0) / x; 40 | for (Eigen::DenseIndex i = 1; i < polynomial.size(); ++i) 41 | val = val * inv_x + polynomial[i]; 42 | 43 | return numext::pow(x, static_cast(polynomial.size() - 1)) * val; 44 | } 45 | } 46 | 47 | } // namespace Eigen 48 | 49 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_EIGEN_BUGFIX_H_ 50 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/base/geometry.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_GEOMETRY_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_GEOMETRY_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | template 41 | using Vector1_ = Eigen::Matrix; ///< 1D Eigen Vector. 42 | template 43 | using Point1_ = Vector1_; ///< 1D Eigen Point. 44 | 45 | template 46 | using Vector2_ = Eigen::Matrix; ///< 2D Eigen Vector. 47 | template 48 | using Point2_ = Vector2_; ///< 2D Eigen Point. 49 | 50 | template 51 | using Translation2_ = Eigen::Translation; ///< 2D Eigen Translation. 52 | 53 | template 54 | using Line2_ = Eigen::Hyperplane; ///< 2D Eigen Line. 55 | 56 | template 57 | using Vector3_ = Eigen::Matrix; ///< 3D Eigen Vector. 58 | template 59 | using Point3_ = Vector3_; ///< 3D Eigen Point. 60 | 61 | template 62 | using Translation3_ = Eigen::Translation; ///< 3D Eigen Translation. 63 | 64 | template 65 | using Transform3_ = Eigen::Transform; ///< 3D Eigen Affine Transform. 66 | template 67 | using Pose3_ = Transform3_; ///< 3D Eigen Pose. 68 | 69 | template 70 | using Quaternion_ = Eigen::Quaternion; ///< Eigen Quaternion. 71 | template 72 | using AngleAxis_ = Eigen::AngleAxis; ///< Eigen AngleAxis angle representation. 73 | 74 | template 75 | using Line3_ = Eigen::ParametrizedLine; ///< 3D Eigen Line. 76 | template 77 | using Plane3_ = Eigen::Hyperplane; ///< 3D Eigen Plane. 78 | 79 | using Vector1 = Vector1_; 80 | using Point1 = Point1_; 81 | 82 | using Vector2 = Vector2_; 83 | using Point2 = Point2_; 84 | 85 | using Translation2 = Translation2_; 86 | 87 | using Line2 = Line2_; 88 | 89 | using Vector3 = Vector3_; 90 | using Point3 = Point3_; 91 | 92 | using Transform3 = Transform3_; 93 | using Pose3 = Pose3_; 94 | 95 | using Quaternion = Quaternion_; 96 | using AngleAxis = AngleAxis_; 97 | using Translation3 = Translation3_; 98 | 99 | using Plane3 = Plane3_; 100 | using Line3 = Line3_; 101 | 102 | 103 | //constexpr Plane3 PlaneXY = Plane3(Vector3::UnitZ(), 0); ///< The $x-y$ plane. 104 | //constexpr Plane3 PlaneXZ = Plane3(Vector3::UnitY(), 0); ///< The $x-z$ plane. 105 | //constexpr Plane3 PlaneYZ = Plane3(Vector3::UnitX(), 0); ///< The $y-z$ plane. 106 | 107 | //Transform3 computeTransform(const Plane3 & from, const Plane3 & to) 108 | //{ 109 | // Quaternion rotation; 110 | // rotation.setFromTwoVectors(from.normal(), to.normal()); 111 | // Translation3 translation(-to.normal() * to.offset()); 112 | // return translation * rotation; 113 | //} 114 | 115 | }// namespace calib 116 | } // namespace unipd 117 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_GEOMETRY_H_ 118 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/base/opencv_eigen_conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013-2014, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_OPENCV_EIGEN_CONVERSIONS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_OPENCV_EIGEN_CONVERSIONS_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | template 41 | cv::Mat_> 42 | eigen2opencv (const Eigen::Matrix & in) 43 | { 44 | auto out = cv::Mat_>(in.cols(), 1); 45 | for (int i = 0; i < in.cols(); ++i) 46 | out(i) = cv::Vec(in.col(i).data()); 47 | return out; 48 | } 49 | 50 | template 51 | EigenT_ 52 | opencv2eigen (const std::vector> & in) 53 | { 54 | auto out = EigenT_(2, in.size()); 55 | for (size_t i = 0; i < in.size(); ++i) 56 | { 57 | const auto & p = in[i]; 58 | out.col(i) << p.x, p.y; 59 | } 60 | return out; 61 | } 62 | 63 | } // namespace calib 64 | } // namespace unipd 65 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_BASE_OPENCV_EIGEN_CONVERSIONS_H_ 66 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/depth/depth.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_DEPTH_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_DEPTH_H_ 31 | 32 | #include "sensor.h" 33 | #include "view.h" 34 | 35 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_DEPTH_H_ 36 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/depth/sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_SENSOR_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_SENSOR_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace unipd 37 | { 38 | namespace calib 39 | { 40 | 41 | class DepthSensor; 42 | class PinholeDepthSensor; 43 | 44 | template <> 45 | struct SensorTraits 46 | { 47 | using Polynomial = PolynomialX_; 48 | }; 49 | 50 | template <> 51 | struct SensorTraits : public SensorTraits 52 | { 53 | using CameraModel = PinholeCameraModel; 54 | using Polynomial = SensorTraits::Polynomial; 55 | }; 56 | 57 | class DepthSensor : public Sensor 58 | { 59 | public: 60 | 61 | DepthSensor () = default; 62 | 63 | DepthSensor (const DepthSensor & other) = default; 64 | 65 | DepthSensor (DepthSensor && other) = default; 66 | 67 | DepthSensor & operator = (const DepthSensor & other) = default; 68 | 69 | DepthSensor & operator = (DepthSensor && other) = default; 70 | 71 | using Sensor::Sensor; 72 | 73 | explicit 74 | DepthSensor (const PolynomialX_ & error_polynomial) 75 | : error_polynomial_(error_polynomial) 76 | { 77 | // Do nothing 78 | } 79 | 80 | const PolynomialX_ & 81 | error () const 82 | { 83 | return error_polynomial_; 84 | } 85 | 86 | void 87 | setError (const PolynomialX_ & error_polynomial) 88 | { 89 | error_polynomial_ = error_polynomial; 90 | } 91 | 92 | void 93 | setError (const PolynomialX_ & error_polynomial, 94 | const std::vector & coefficients) 95 | { 96 | assert(error_polynomial.size() == coefficients.size()); 97 | error_polynomial_ = error_polynomial; 98 | error_polynomial_.setCoefficients(PolynomialX_::Coefficients::Map(coefficients.data(), coefficients.size())); 99 | } 100 | 101 | Scalar 102 | estimateError (const Scalar & x) 103 | { 104 | return error_polynomial_.evaluate(x); 105 | } 106 | 107 | private: 108 | 109 | PolynomialX_ error_polynomial_; 110 | 111 | }; 112 | 113 | class PinholeDepthSensor : public DepthSensor 114 | { 115 | public: 116 | 117 | PinholeDepthSensor () = default; 118 | 119 | PinholeDepthSensor (const PinholeDepthSensor & other) = default; 120 | 121 | PinholeDepthSensor (PinholeDepthSensor && other) = default; 122 | 123 | PinholeDepthSensor & operator = (const PinholeDepthSensor & other) = default; 124 | 125 | PinholeDepthSensor & operator = (PinholeDepthSensor && other) = default; 126 | 127 | using DepthSensor::DepthSensor; 128 | 129 | const PinholeCameraModel & 130 | cameraModel () const 131 | { 132 | return camera_model_; 133 | } 134 | 135 | void 136 | setCameraModel (const PinholeCameraModel & camera_model) 137 | { 138 | camera_model_ = camera_model; 139 | } 140 | 141 | void 142 | setCameraModel (PinholeCameraModel && camera_model) 143 | { 144 | camera_model_ = camera_model; 145 | } 146 | 147 | private: 148 | 149 | PinholeCameraModel camera_model_; 150 | 151 | }; 152 | 153 | } // namespace calib 154 | } // namespace unipd 155 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_DEPTH_SENSOR_H_ 156 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/depth/view.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATON_COMMON_DEPTH_VIEW_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATON_COMMON_DEPTH_VIEW_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | template 41 | class DepthView : public IndicesView_, Indices1> 42 | { 43 | public: 44 | 45 | using Base = IndicesView_, Indices1>; 46 | // using Point = typename Base::Point; 47 | 48 | virtual 49 | ~DepthView () 50 | { 51 | // Do nothing 52 | } 53 | 54 | // protected: 55 | 56 | // inline virtual Point 57 | // computeCentroid () const 58 | // { 59 | // const auto & points = Base::indices(); 60 | // const auto & cloud = *Base::data(); 61 | // assert(not points.empty()); 62 | // Point sum = Point::Zero(); 63 | // for (Size1 i = 0; i < points.size(); ++i) 64 | // sum += cloud[points[i]]; 65 | // sum /= points.size(); 66 | // return sum; 67 | // } 68 | 69 | }; 70 | 71 | } // namespace calib 72 | } // namespace unipd 73 | 74 | #endif // UNIPD_CALIBRATION_CALIBRATON_COMMON_DEPTH_VIEW_H_ 75 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/objects/base_object.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_BASE_OBJECT_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_BASE_OBJECT_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | class BaseObject 41 | { 42 | public: 43 | 44 | BaseObject () = default; 45 | 46 | BaseObject (const BaseObject & other) = default; 47 | 48 | BaseObject (BaseObject && other) = default; 49 | 50 | BaseObject & operator = (const BaseObject & other) = default; 51 | 52 | BaseObject & operator = (BaseObject && other) = default; 53 | 54 | explicit 55 | BaseObject (const std::string & frame_id) 56 | : frame_id_(frame_id) 57 | { 58 | // Do nothing 59 | } 60 | 61 | virtual 62 | ~BaseObject () 63 | { 64 | // Do nothing 65 | } 66 | 67 | virtual void 68 | transform (const Transform3 & transform) 69 | { 70 | pose_ = transform * pose_; 71 | } 72 | 73 | const Pose3 & 74 | pose () const 75 | { 76 | return pose_; 77 | } 78 | 79 | const std::string & 80 | frameId () const 81 | { 82 | return frame_id_; 83 | } 84 | 85 | const std::shared_ptr & 86 | parent () const 87 | { 88 | return parent_; 89 | } 90 | 91 | bool 92 | hasParent () const 93 | { 94 | return static_cast(parent_); 95 | } 96 | 97 | void 98 | setFrameId (const std::string & frame_id) 99 | { 100 | frame_id_ = frame_id; 101 | } 102 | 103 | void 104 | setParent (const std::shared_ptr & parent) 105 | { 106 | parent_ = parent; 107 | } 108 | 109 | virtual void 110 | reset () 111 | { 112 | pose_ = Pose3::Identity(); 113 | parent_.reset(); 114 | } 115 | 116 | virtual void 117 | setPose (const Pose3 & pose) 118 | { 119 | pose_ = pose; 120 | } 121 | 122 | friend std::ostream & 123 | operator << (std::ostream & stream, 124 | const BaseObject & object) 125 | { 126 | stream << "frame_id: " << object.frame_id_ << ", " 127 | << "parent: " << object.parent_ << " (= " << (object.hasParent() ? object.parent_->frameId() : "null") << "), " 128 | << "pose: {" 129 | << "rotation: [" << Quaternion(object.pose_.rotation()).coeffs().transpose() << "], " 130 | << "translation: [" << object.pose_.translation().transpose() << "]}"; 131 | return stream; 132 | } 133 | 134 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 135 | 136 | private: 137 | 138 | Pose3 pose_ = Pose3::Identity(); 139 | std::string frame_id_; 140 | std::shared_ptr parent_; 141 | 142 | }; 143 | 144 | } // namespace calib 145 | } // namespace unipd 146 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_BASE_OBJECT_H_ 147 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/objects/objects.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_OBJECTS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_OBJECTS_H_ 31 | 32 | #include "base_object.h" 33 | #include "checkerboard.h" 34 | #include "planar_object.h" 35 | #include "sensor.h" 36 | #include "view.h" 37 | 38 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_OBJECTS_H_ 39 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/objects/planar_object.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_PLANAR_OBJECT_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_PLANAR_OBJECT_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | /** 40 | * @brief The PlanarObject class 41 | */ 42 | class PlanarObject : public BaseObject 43 | { 44 | public: 45 | 46 | PlanarObject (const PlanarObject & other) = default; 47 | 48 | PlanarObject (PlanarObject && other) = default; 49 | 50 | PlanarObject & operator = (const PlanarObject & other) = default; 51 | 52 | PlanarObject & operator = (PlanarObject && other) = default; 53 | 54 | explicit 55 | PlanarObject (const Plane3 & plane) 56 | : BaseObject(), 57 | plane_(plane), 58 | initial_plane_(plane) 59 | { 60 | // Do nothing 61 | } 62 | 63 | PlanarObject (const std::string & frame_id, 64 | const Plane3 & plane) 65 | : BaseObject(frame_id), 66 | plane_(plane), 67 | initial_plane_(plane) 68 | { 69 | // Do nothing 70 | } 71 | 72 | virtual 73 | ~PlanarObject () 74 | { 75 | // Do nothing 76 | } 77 | 78 | virtual void 79 | reset () override 80 | { 81 | BaseObject::reset(); 82 | plane_ = initial_plane_; 83 | } 84 | 85 | virtual void 86 | setPose (const Pose3 & pose) override 87 | { 88 | BaseObject::setPose(pose); 89 | plane_ = initial_plane_; 90 | plane_.transform(pose); 91 | } 92 | 93 | virtual void 94 | transform (const Transform3 & transform) override 95 | { 96 | BaseObject::transform(transform); 97 | plane_.transform(transform); 98 | } 99 | 100 | const Plane3 & 101 | plane () const 102 | { 103 | return plane_; 104 | } 105 | 106 | private: 107 | 108 | Plane3 plane_; 109 | const Plane3 initial_plane_; 110 | 111 | }; 112 | 113 | } // namespace calib 114 | } // namespace unipd 115 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_PLANAR_OBJECT_H_ 116 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/objects/sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_SENSOR_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_SENSOR_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | template 40 | struct SensorTraits {}; 41 | 42 | class Sensor : public BaseObject 43 | { 44 | public: 45 | 46 | using BaseObject::BaseObject; 47 | 48 | }; 49 | 50 | } // namespace calib 51 | } // namespace unipd 52 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_SENSOR_H_ 53 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/objects/with_points.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_WITH_POINTS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_WITH_POINTS_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | struct WithPoints 40 | { 41 | virtual 42 | ~WithPoints () 43 | { 44 | // Do nothing 45 | } 46 | 47 | virtual const Cloud3 & 48 | points () const = 0; 49 | 50 | }; 51 | 52 | } // namespace calib 53 | } // namespace unipd 54 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_OBJECTS_WITH_POINTS_H_ 55 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/pinhole/camera_model.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_CAMERA_MODEL_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_CAMERA_MODEL_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | class PinholeCameraModel : public image_geometry::PinholeCameraModel 41 | { 42 | public: 43 | 44 | typedef image_geometry::PinholeCameraModel Base; 45 | 46 | PinholeCameraModel () = default; 47 | 48 | PinholeCameraModel (const PinholeCameraModel & other) = default; 49 | 50 | PinholeCameraModel (PinholeCameraModel && other) = default; 51 | 52 | PinholeCameraModel & operator = (const PinholeCameraModel & other) = default; 53 | 54 | PinholeCameraModel & operator = (PinholeCameraModel && other) = default; 55 | 56 | explicit 57 | PinholeCameraModel (const sensor_msgs::CameraInfo & msg) 58 | : PinholeCameraModel() 59 | { 60 | Base::fromCameraInfo(msg); 61 | 62 | f_ << fx(), fy(); 63 | f_inv_ << 1.0 / fx(), 1.0 / fy(); 64 | c_ << cx(), cy(); 65 | T_ << Tx(), Ty(); 66 | p_ << D_(0, 2), D_(0, 3); 67 | k_ << D_(0, 0), D_(0, 1), D_(0, 4), D_(0, 5), D_(0, 6), D_(0, 7); 68 | R_ = Eigen::Map>(&Base::R_(0, 0)); 69 | } 70 | 71 | template 72 | Point3_ 73 | projectPixelTo3dRay (const Point2_ & pixel_point) const; 74 | 75 | template 76 | Point2_ 77 | project3dToPixel (const Point3_ & world_point) const; 78 | 79 | template 80 | void 81 | projectPixelTo3dRay (const Cloud2_ & pixel_points, 82 | Cloud3_ & world_points) const; 83 | 84 | template 85 | Cloud3_ 86 | projectPixelTo3dRay (const Cloud2_ & pixel_points) const; 87 | 88 | template 89 | void 90 | project3dToPixel (const Cloud3_ & world_points, 91 | Cloud2_ & pixel_points) const; 92 | 93 | template 94 | Cloud2_ 95 | project3dToPixel (const Cloud3_ & world_points) const; 96 | 97 | template 98 | Point2_ 99 | distortPoint (const Point2_ & pixel_point) const; 100 | 101 | template 102 | Cloud2_ 103 | distortPoints (const Cloud2_ & pixel_points) const; 104 | 105 | template 106 | Pose3_ 107 | estimatePose (const Cloud2_ & points_image, 108 | const Cloud3_ & points_object) const; 109 | 110 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 111 | 112 | private: 113 | 114 | Eigen::Matrix f_; 115 | Eigen::Matrix f_inv_; 116 | Eigen::Matrix c_; 117 | Eigen::Matrix T_; 118 | Eigen::Matrix p_; 119 | Eigen::Matrix k_; 120 | Eigen::Matrix R_; 121 | 122 | }; 123 | 124 | } // namespace calib 125 | } // namespace unipd 126 | 127 | #include 128 | 129 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_CAMERA_MODEL_H_ 130 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/pinhole/pinhole.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_PINHOLE_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_PINHOLE_H_ 31 | 32 | #include "camera_model.h" 33 | #include "sensor.h" 34 | #include "view.h" 35 | 36 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_PINHOLE_H_ 37 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/pinhole/sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_SENSOR_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_SENSOR_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | class PinholeSensor; 41 | 42 | template <> 43 | struct SensorTraits 44 | { 45 | using CameraModel = PinholeCameraModel; 46 | }; 47 | 48 | class PinholeSensor : public Sensor 49 | { 50 | public: 51 | 52 | PinholeSensor () = default; 53 | 54 | PinholeSensor (const PinholeSensor & other) = default; 55 | 56 | PinholeSensor (PinholeSensor && other) = default; 57 | 58 | PinholeSensor & operator = (const PinholeSensor & other) = default; 59 | 60 | PinholeSensor & operator = (PinholeSensor && other) = default; 61 | 62 | using Sensor::Sensor; 63 | 64 | const PinholeCameraModel & 65 | cameraModel () const 66 | { 67 | return camera_model_; 68 | } 69 | 70 | void 71 | setCameraModel (const PinholeCameraModel & camera_model) 72 | { 73 | camera_model_ = camera_model; 74 | } 75 | 76 | void 77 | setCameraModel (PinholeCameraModel && camera_model) 78 | { 79 | camera_model_ = camera_model; 80 | } 81 | 82 | template 83 | inline Pose3_ 84 | estimatePose (const Cloud2_ & points_image, 85 | const Cloud3_ & points_object) const 86 | { 87 | return camera_model_.estimatePose(points_image, points_object); 88 | } 89 | 90 | private: 91 | 92 | PinholeCameraModel camera_model_; 93 | 94 | }; 95 | 96 | } // namespace calib 97 | } // namespace unipd 98 | #endif // UNIPD_CALIBRATION_CALIBRATION_COMMON_PINHOLE_SENSOR_H_ 99 | -------------------------------------------------------------------------------- /calibration_common/include/calibration_common/pinhole/view.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATON_COMMON_PINHOLE_VIEW_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATON_COMMON_PINHOLE_VIEW_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace cv 36 | { 37 | class Mat; 38 | } 39 | 40 | namespace unipd 41 | { 42 | namespace calib 43 | { 44 | 45 | template 46 | class PinholeView : public IndicesView_ 47 | { 48 | public: 49 | 50 | using Base = IndicesView_; 51 | using Point = typename Base::Point; 52 | 53 | using Base::Base; 54 | 55 | virtual 56 | ~PinholeView () 57 | { 58 | // Do nothing 59 | } 60 | 61 | }; 62 | 63 | template 64 | class PinholePointsView : public PointsView_ 65 | { 66 | public: 67 | 68 | using Base = PointsView_; 69 | using Point = typename Base::Point; 70 | 71 | using Base::Base; 72 | 73 | virtual 74 | ~PinholePointsView () 75 | { 76 | // Do nothing 77 | } 78 | 79 | protected: 80 | 81 | inline virtual Point 82 | computeCentroid () const 83 | { 84 | return Base::points().container().rowwise().mean(); 85 | } 86 | 87 | }; 88 | 89 | } // namespace calib 90 | } // namespace unipd 91 | #endif // UNIPD_CALIBRATION_CALIBRATON_COMMON_PINHOLE_VIEW_H_ 92 | -------------------------------------------------------------------------------- /calibration_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | calibration_common 5 | 1.0.0 6 | 7 | The calibration_common package 8 | 9 | Filippo Basso 10 | BSD 11 | 12 | 13 | 14 | catkin 15 | 16 | cmake_modules 17 | image_geometry 18 | 19 | eigen 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /calibration_common/test/base/test_opencv_eigen_conversions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | TEST(opencv_eigen, eigen2opencv) 33 | { 34 | Eigen::Matrix in; 35 | in.col(0) = Eigen::Vector3d(1, 2, 3); 36 | in.col(1) = Eigen::Vector3d(4, 5, 6); 37 | 38 | cv::Mat_ out = unipd::calib::eigen2opencv(in); 39 | 40 | for (int i = 0; i < 3; ++i) 41 | for (int j = 0; j < 2; ++j) 42 | EXPECT_EQ(in(i, j), out.at(j)[i]); 43 | 44 | } 45 | 46 | TEST(opencv_eigen, opencv2eigen) 47 | { 48 | std::vector in(3); // (rows, cols) 49 | in[0] = cv::Point2d(1, 2); 50 | in[1] = cv::Point2d(3, 4); 51 | in[2] = cv::Point2d(5, 6); 52 | 53 | auto out = unipd::calib::opencv2eigen>(in); 54 | 55 | for (int i = 0; i < 3; ++i) 56 | { 57 | EXPECT_EQ(out(0, i), in[i].x); 58 | EXPECT_EQ(out(1, i), in[i].y); 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /calibration_common/test/base/test_polynomial.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | using namespace unipd::calib; 33 | 34 | TEST(Polynomial_, Polynomial_) 35 | { 36 | Polynomial_ polynomial1; 37 | Polynomial_ polynomial2; 38 | Polynomial_ polynomial3; 39 | SUCCEED(); 40 | } 41 | 42 | TEST(PolynomialX_, Polynomial_1) 43 | { 44 | PolynomialX_ polynomial(4, 2); 45 | EXPECT_EQ(polynomial.maxDegree(), 4); 46 | EXPECT_EQ(polynomial.minDegree(), 2); 47 | EXPECT_EQ(polynomial.size(), 3); 48 | } 49 | 50 | TEST(PolynomialX_, PolynomialX_2) 51 | { 52 | ::testing::FLAGS_gtest_death_test_style = "threadsafe"; 53 | PolynomialX_ polynomial; 54 | EXPECT_DEATH(polynomial.maxDegree(), "initialized_"); 55 | EXPECT_DEATH(polynomial.minDegree(), "initialized_"); 56 | EXPECT_DEATH(polynomial.size(), "initialized_"); 57 | } 58 | 59 | TEST(Polynomial_, identityCoefficients) 60 | { 61 | Polynomial_ polynomial; 62 | auto coeffs = polynomial.identityCoefficients(); 63 | for (int i = 0; i < Polynomial_::Size; ++i) 64 | EXPECT_EQ(coeffs[i], i == 1 ? 1.0 : 0.0); 65 | } 66 | 67 | TEST(PolynomialX_, identityCoefficients) 68 | { 69 | PolynomialX_ polynomial(3, 0); 70 | auto coeffs = polynomial.identityCoefficients(); 71 | for (int i = 0; i < polynomial.size(); ++i) 72 | EXPECT_EQ(coeffs[i], i == 1 ? 1.0 : 0.0); 73 | } 74 | 75 | TEST(PolynomialX_, identityCoefficients_death) 76 | { 77 | ::testing::FLAGS_gtest_death_test_style = "threadsafe"; 78 | PolynomialX_ polynomial(3, 2); 79 | EXPECT_DEATH(polynomial.identityCoefficients(), "min_degree_ <= 1"); 80 | } 81 | 82 | TEST(Polynomial_, createCoefficients) 83 | { 84 | Polynomial_ polynomial; 85 | auto coeffs = polynomial.createCoefficients(); 86 | EXPECT_EQ(coeffs.SizeAtCompileTime, 3); 87 | } 88 | 89 | TEST(PolynomialX_, createCoefficients) 90 | { 91 | PolynomialX_ polynomial(4, 1); 92 | auto coeffs = polynomial.createCoefficients(); 93 | EXPECT_EQ(coeffs.size(), 4); 94 | } 95 | 96 | TEST(PolynomialX_, createCoefficients_death) 97 | { 98 | ::testing::FLAGS_gtest_death_test_style = "threadsafe"; 99 | PolynomialX_ polynomial; 100 | EXPECT_DEATH(polynomial.createCoefficients(), "initialized_"); 101 | } 102 | 103 | TEST(Polynomial_, evaluate) 104 | { 105 | Polynomial_ polynomial; 106 | auto coeffs = Polynomial_::Coefficients(); 107 | coeffs[0] = 4; 108 | coeffs[1] = 3; 109 | coeffs[2] = 2; 110 | coeffs[3] = 1; 111 | auto x = 1.5; 112 | auto y = coeffs[0] + coeffs[1] * x + coeffs[2] * x * x + coeffs[3] * x * x * x; 113 | EXPECT_NEAR(polynomial.evaluate(coeffs, x), y, 1e-5); 114 | } 115 | 116 | TEST(PolynomialX_, evaluate) 117 | { 118 | PolynomialX_ polynomial(3, 1); 119 | auto coeffs = polynomial.createCoefficients(); 120 | coeffs[0] = 4; 121 | coeffs[1] = 3; 122 | coeffs[2] = 2; 123 | auto x = 2.5; 124 | auto y = coeffs[0] * x + coeffs[1] * x * x + coeffs[2] * x * x * x; 125 | EXPECT_NEAR(polynomial.evaluate(coeffs, x), y, 1e-5); 126 | } 127 | -------------------------------------------------------------------------------- /calibration_common/test/objects/test_checkerboard.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace unipd::calib; 34 | using sensor_msgs::CameraInfo; 35 | 36 | TEST(Checkerboard, Checkerboard_1) 37 | { 38 | Checkerboard(2, 3, 0.1, 0.2); 39 | Checkerboard("cb", 2, 3, 0.1, 0.2); 40 | SUCCEED(); 41 | } 42 | 43 | TEST(Checkerboard, Checkerboard_2) 44 | { 45 | std::shared_ptr original = std::make_shared(2, 3, 0.1, 0.2); 46 | 47 | CameraInfo camera_info = CameraInfo(); 48 | camera_info.width = 640; 49 | camera_info.height = 480; 50 | camera_info.distortion_model = "plumb_bob"; 51 | camera_info.K = CameraInfo::_K_type{200.0, 0.0, 320.0, 0.0, 200.0, 240.0, 0.0, 0.0, 1.0}; 52 | camera_info.P = CameraInfo::_P_type{200.0, 0.0, 320.0, 0.0, 0.0, 200.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; 53 | camera_info.D = CameraInfo::_D_type{0.0, 0.0, 0.0, 0.0, 0.0}; 54 | 55 | std::shared_ptr sensor = std::make_shared(); 56 | sensor->setCameraModel(PinholeCameraModel(camera_info)); 57 | 58 | Checkerboard moved = *original; 59 | moved.transform(Transform3::Identity() * Translation3(Vector3::UnitZ())); 60 | 61 | Cloud2 corners_image = sensor->cameraModel().project3dToPixel(moved.corners()); 62 | 63 | PinholePointsView view; 64 | view.setObject(original); 65 | view.setPoints(corners_image); 66 | view.setSensor(sensor); 67 | 68 | Checkerboard created = Checkerboard(view); 69 | 70 | EXPECT_TRUE(created.corners().container().isApprox(moved.corners().container(), 1e-5)); 71 | } 72 | 73 | TEST(Checkerboard, pose) 74 | { 75 | auto cb = Checkerboard(2, 3, 0.1, 0.2); 76 | EXPECT_TRUE((cb.pose().matrix().array() == Pose3::Identity().matrix().array()).all()); 77 | } 78 | 79 | TEST(Checkerboard, frame_id) 80 | { 81 | auto cb = Checkerboard("cb", 2, 3, 0.1, 0.2); 82 | EXPECT_EQ(cb.frameId(), "cb"); 83 | } 84 | 85 | TEST(Checkerboard, parent) 86 | { 87 | auto cb = Checkerboard(2, 3, 0.1, 0.2); 88 | EXPECT_FALSE(cb.parent()); 89 | } 90 | 91 | TEST(Checkerboard, plane) 92 | { 93 | auto cb = Checkerboard(2, 3, 0.1, 0.2); 94 | EXPECT_TRUE((cb.plane().coeffs().array() == Plane3(Vector3::UnitZ(), 0).coeffs().array()).all()); 95 | } 96 | 97 | TEST(Checkerboard, cols) 98 | { 99 | auto cb = Checkerboard(2, 3, 0.1, 0.2); 100 | EXPECT_EQ(cb.cols(), 2); 101 | } 102 | 103 | TEST(Checkerboard, rows) 104 | { 105 | auto cb = Checkerboard(2, 3, 0.1, 0.2); 106 | EXPECT_EQ(cb.rows(), 3); 107 | } 108 | -------------------------------------------------------------------------------- /calibration_common/test/test_calibration_common.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | 31 | int 32 | main(int argc, char ** argv) 33 | { 34 | ::testing::InitGoogleTest(&argc, argv); 35 | return RUN_ALL_TESTS(); 36 | } 37 | -------------------------------------------------------------------------------- /calibration_msgs/action/CheckerboardExtraction.action: -------------------------------------------------------------------------------- 1 | #goal 2 | 3 | uint8 EXTRACT_FROM_NONE=0 4 | uint8 EXTRACT_FROM_IMAGE=1 5 | uint8 EXTRACT_FROM_DEPTH=2 6 | uint8 EXTRACT_FROM_ALL=3 7 | 8 | int32 ALL_POINTS=-1 9 | int32 PLANE_ONLY=0 10 | 11 | Header header 12 | 13 | Checkerboard checkerboard 14 | uint8 extract_from 15 | int32 max_depth_points 16 | 17 | # max_depth_points must be in the interval [3, +inf[ 18 | # or be one of the special values ALL_POINTS, PLANE_ONLY 19 | 20 | --- 21 | 22 | #result 23 | 24 | calibration_msgs/Point2DArray[] image_corners 25 | calibration_msgs/PointArray[] depth_points 26 | 27 | # If depth_points[i] size is 2: 28 | # - depth_points[i][0] = point on the plane 29 | # - depth_points[i][1] = plane normal 30 | 31 | --- 32 | 33 | #feedback 34 | -------------------------------------------------------------------------------- /calibration_msgs/include/calibration_msgs/calibration_common_conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_MSGS_CALIBRATION_COMMON_CONVERSIONS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_MSGS_CALIBRATION_COMMON_CONVERSIONS_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace unipd 42 | { 43 | namespace calib 44 | { 45 | 46 | Checkerboard 47 | fromMessage (const calibration_msgs::Checkerboard & msg); 48 | 49 | Checkerboard 50 | fromMessage (const calibration_msgs::Checkerboard & msg, 51 | const std::shared_ptr & parent); 52 | 53 | calibration_msgs::Checkerboard 54 | toMessage (const Checkerboard & checkerboard); 55 | 56 | Cloud2 57 | fromMessage (const calibration_msgs::Point2DArray & msg); 58 | 59 | Cloud3 60 | fromMessage (const calibration_msgs::PointArray & msg); 61 | 62 | const calibration_msgs::Point2DArray 63 | toMessage (const Cloud2 & points); 64 | 65 | const calibration_msgs::PointArray 66 | toMessage (const Cloud3 & points); 67 | 68 | template 69 | std::shared_ptr 70 | fromMessage (const sensor_msgs::CameraInfo & msg) 71 | { 72 | auto sensor = std::make_shared(msg.header.frame_id); 73 | sensor->setCameraModel(typename SensorTraits::CameraModel(msg)); 74 | return sensor; 75 | } 76 | 77 | template 78 | calibration_msgs::Polynomial 79 | toMessage (const PolynomialT_ & polynomial, 80 | const CoefficientsT_ & coefficients) 81 | { 82 | assert(coefficients.size() == polynomial.size()); 83 | calibration_msgs::Polynomial msg; 84 | msg.max_degree = polynomial.maxDegree(); 85 | msg.min_degree = polynomial.minDegree(); 86 | msg.coefficients.resize(coefficients.size()); 87 | for (decltype(coefficients.size()) i = 0; i < coefficients.size(); ++i) 88 | msg.coefficients[i] = coefficients[i]; 89 | return msg; 90 | } 91 | 92 | template 93 | PolynomialX_ 94 | fromMessage (const calibration_msgs::Polynomial & msg) 95 | { 96 | assert(msg.coefficients.size() == computePolynomialSize(msg.max_degree, msg.min_degree)); 97 | PolynomialX_ poly = PolynomialX_(msg.max_degree, msg.min_degree); 98 | poly.setCoefficients(PolynomialX_::Coefficients::Map(msg.coefficients.data(), msg.coefficients.size()).template cast()); 99 | return poly; 100 | } 101 | 102 | 103 | } // namespace calib 104 | } // namespace unipd 105 | 106 | #endif // UNIPD_CALIBRATION_CALIBRATION_MSGS_CALIBRATION_COMMON_CONVERSIONS_H_ 107 | -------------------------------------------------------------------------------- /calibration_msgs/msg/CalibrationPose.msg: -------------------------------------------------------------------------------- 1 | string frame_id 2 | 3 | string child_frame_id 4 | geometry_msgs/Pose pose 5 | -------------------------------------------------------------------------------- /calibration_msgs/msg/Checkerboard.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string parent_frame_id 4 | geometry_msgs/Pose pose 5 | 6 | uint32 rows 7 | uint32 cols 8 | float32 cell_width 9 | float32 cell_height 10 | -------------------------------------------------------------------------------- /calibration_msgs/msg/CheckerboardCornersView.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string view_id 4 | string checkerboard_frame_id 5 | Point2D[] points 6 | -------------------------------------------------------------------------------- /calibration_msgs/msg/CheckerboardDepthView.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string view_id 4 | string checkerboard_frame_id 5 | geometry_msgs/Vector3[] points 6 | 7 | # If points size is 2: 8 | # - points[0] = point on the plane 9 | # - points[1] = plane normal 10 | -------------------------------------------------------------------------------- /calibration_msgs/msg/Point2D.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | -------------------------------------------------------------------------------- /calibration_msgs/msg/Point2DArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 x_size 4 | uint32 y_size 5 | calibration_msgs/Point2D[] points 6 | -------------------------------------------------------------------------------- /calibration_msgs/msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 x_size 4 | uint32 y_size 5 | geometry_msgs/Point[] points 6 | -------------------------------------------------------------------------------- /calibration_msgs/msg/Polynomial.msg: -------------------------------------------------------------------------------- 1 | int32 max_degree 2 | int32 min_degree 3 | float64[] coefficients 4 | -------------------------------------------------------------------------------- /calibration_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | calibration_msgs 5 | 1.0.0 6 | 7 | The calibration_msgs package 8 | 9 | Filippo Basso 10 | BSD 11 | 12 | 13 | 14 | catkin 15 | 16 | message_generation 17 | message_runtime 18 | 19 | sensor_msgs 20 | actionlib_msgs 21 | calibration_common 22 | eigen_conversions 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /calibration_msgs/src/calibration_msgs/calibration_common_conversions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | namespace unipd 33 | { 34 | namespace calib 35 | { 36 | 37 | Checkerboard 38 | fromMessage (const calibration_msgs::Checkerboard & msg) 39 | { 40 | assert (msg.parent_frame_id == ""); 41 | return Checkerboard(msg.header.frame_id, msg.cols, msg.rows, msg.cell_width, msg.cell_height); 42 | } 43 | 44 | Checkerboard 45 | fromMessage (const calibration_msgs::Checkerboard & msg, 46 | const std::shared_ptr & parent) 47 | { 48 | assert (msg.parent_frame_id == parent->frameId()); 49 | Checkerboard checkerboard = Checkerboard(msg.header.frame_id, msg.cols, msg.rows, msg.cell_width, msg.cell_height); 50 | checkerboard.setParent(parent); 51 | Pose3 pose; 52 | tf::poseMsgToEigen(msg.pose, pose); 53 | checkerboard.setPose(pose); 54 | return checkerboard; 55 | } 56 | 57 | calibration_msgs::Checkerboard 58 | toMessage (const Checkerboard & checkerboard) 59 | { 60 | calibration_msgs::Checkerboard msg; 61 | msg.header.frame_id = checkerboard.frameId(); 62 | if (checkerboard.hasParent()) 63 | msg.parent_frame_id = checkerboard.parent()->frameId(); 64 | msg.rows = checkerboard.rows(); 65 | msg.cols = checkerboard.cols(); 66 | msg.cell_width = checkerboard.cellWidth(); 67 | msg.cell_height = checkerboard.cellHeight(); 68 | tf::poseEigenToMsg(checkerboard.pose(), msg.pose); 69 | return msg; 70 | } 71 | 72 | Cloud2 73 | fromMessage (const calibration_msgs::Point2DArray & msg) 74 | { 75 | Cloud2 points(Size2{msg.x_size, msg.y_size}); 76 | for (int i = 0; i < msg.points.size(); ++i) 77 | points[i] << msg.points[i].x, msg.points[i].y; 78 | return points; 79 | } 80 | 81 | Cloud3 82 | fromMessage (const calibration_msgs::PointArray & msg) 83 | { 84 | Cloud3 points(Size2{msg.x_size, msg.y_size}); 85 | for (int i = 0; i < msg.points.size(); ++i) 86 | points[i] << msg.points[i].x, msg.points[i].y, msg.points[i].z; 87 | return points; 88 | } 89 | 90 | const calibration_msgs::Point2DArray 91 | toMessage (const Cloud2 & points) 92 | { 93 | calibration_msgs::Point2DArray msg; 94 | msg.x_size = points.size().x; 95 | msg.y_size = points.size().y; 96 | msg.points.resize(msg.x_size * msg.y_size); 97 | for (int i = 0; i < msg.points.size(); ++i) 98 | { 99 | msg.points[i].x = points[i].x(); 100 | msg.points[i].y = points[i].y(); 101 | } 102 | return msg; 103 | } 104 | 105 | const calibration_msgs::PointArray 106 | toMessage (const Cloud3 & points) 107 | { 108 | calibration_msgs::PointArray msg; 109 | msg.x_size = points.size().x; 110 | msg.y_size = points.size().y; 111 | msg.points.resize(msg.x_size * msg.y_size); 112 | for (int i = 0; i < msg.points.size(); ++i) 113 | { 114 | msg.points[i].x = points[i].x(); 115 | msg.points[i].y = points[i].y(); 116 | msg.points[i].z = points[i].z(); 117 | } 118 | return msg; 119 | } 120 | 121 | } // namespace calib 122 | } // namespace unipd 123 | -------------------------------------------------------------------------------- /calibration_msgs/srv/GetCalibrationResults.srv: -------------------------------------------------------------------------------- 1 | 2 | 3 | --- 4 | 5 | calibration_msgs/CalibrationPose[] poses 6 | 7 | -------------------------------------------------------------------------------- /calibration_msgs/srv/GetDeviceInfo.srv: -------------------------------------------------------------------------------- 1 | uint8 INTENSITY=1 2 | uint8 DEPTH=2 3 | uint8 ALL=3 4 | 5 | uint8 requested_types 6 | 7 | --- 8 | 9 | uint8 UNDEFINED=0 10 | uint8 INTENSITY=1 11 | uint8 DEPTH=2 12 | 13 | uint8[] sensor_types 14 | sensor_msgs/CameraInfo[] camera_infos 15 | Polynomial[] error_polynomials 16 | 17 | geometry_msgs/TransformStamped[] initial_transforms 18 | geometry_msgs/TransformStamped[] fixed_transforms 19 | 20 | -------------------------------------------------------------------------------- /calibration_pcl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibration_pcl) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | calibration_common 11 | ) 12 | find_package(Boost REQUIRED 13 | COMPONENTS 14 | system 15 | signals 16 | ) 17 | find_package(PCL 1.7 REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | ## The catkin_package macro generates cmake config files for your package 23 | ## Declare things to be passed to dependent projects 24 | ## INCLUDE_DIRS: uncomment this if you package contains header files 25 | ## LIBRARIES: libraries you create in this project that dependent projects also need 26 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 27 | ## DEPENDS: system dependencies of this project that dependent projects also need 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | # LIBRARIES ${PROJECT_NAME} 31 | CATKIN_DEPENDS 32 | calibration_common 33 | DEPENDS 34 | Boost 35 | PCL 36 | ) 37 | 38 | ########### 39 | ## Build ## 40 | ########### 41 | 42 | add_custom_target(${PROJECT_NAME}_HEADERS 43 | SOURCES 44 | include/${PROJECT_NAME}/base/base.h 45 | include/${PROJECT_NAME}/base/definitions.h 46 | include/${PROJECT_NAME}/base/pcl_eigen_conversions.h 47 | 48 | include/${PROJECT_NAME}/depth/view.h 49 | ) 50 | 51 | ## Specify additional locations of header files 52 | ## Your package locations should be listed before other locations 53 | include_directories( 54 | include 55 | ${catkin_INCLUDE_DIRS} 56 | ${Boost_INCLUDE_DIRS} 57 | ${PCL_INCLUDE_DIRS} 58 | ) 59 | 60 | ## Declare a cpp library 61 | #add_library(calibration_pcl 62 | # src/${PROJECT_NAME}/calibration_pcl.cpp 63 | #) 64 | 65 | 66 | ## Declare a cpp executable 67 | #add_executable(dummy src/dummy.cpp) 68 | 69 | ## Add cmake target dependencies of the executable/library 70 | ## as an example, message headers may need to be generated before nodes 71 | # add_dependencies(calibration_pcl_node calibration_pcl_generate_messages_cpp) 72 | 73 | ## Specify libraries to link a library or executable target against 74 | #target_link_libraries(checkerboard_corners_extraction 75 | # ${catkin_LIBRARIES} 76 | #) 77 | 78 | ############# 79 | ## Install ## 80 | ############# 81 | 82 | # all install targets should use catkin DESTINATION variables 83 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 84 | 85 | ## Mark executable scripts (Python etc.) for installation 86 | ## in contrast to setup.py, you can choose the destination 87 | # install(PROGRAMS 88 | # scripts/my_python_script 89 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 90 | # ) 91 | 92 | ## Mark executables and/or libraries for installation 93 | # install(TARGETS calibration_pcl calibration_pcl_node 94 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 97 | # ) 98 | 99 | ## Mark cpp header files for installation 100 | # install(DIRECTORY include/${PROJECT_NAME}/ 101 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 102 | # FILES_MATCHING PATTERN "*.h" 103 | # PATTERN ".svn" EXCLUDE 104 | # ) 105 | 106 | ## Mark other files for installation (e.g. launch and bag files, etc.) 107 | # install(FILES 108 | # # myfile1 109 | # # myfile2 110 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 111 | # ) 112 | 113 | ############# 114 | ## Testing ## 115 | ############# 116 | 117 | ## Add gtest based cpp test target and link libraries 118 | catkin_add_gtest(${PROJECT_NAME}-test 119 | test/test_${PROJECT_NAME}.cpp 120 | test/base/test_pcl_eigen_conversions.cpp 121 | ) 122 | 123 | if(TARGET ${PROJECT_NAME}-test) 124 | target_link_libraries(${PROJECT_NAME}-test 125 | ${PCL_LIBS} 126 | ) 127 | endif() 128 | 129 | ## Add folders to be run by python nosetests 130 | # catkin_add_nosetests(test) 131 | -------------------------------------------------------------------------------- /calibration_pcl/include/calibration_pcl/base/base.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_BASE_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_BASE_H_ 31 | 32 | #include "definitions.h" 33 | #include "pcl_eigen_conversions.h" 34 | 35 | #endif // UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_BASE_H_ 36 | -------------------------------------------------------------------------------- /calibration_pcl/include/calibration_pcl/base/definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_DEFINITIONS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_DEFINITIONS_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | using PCLPoint3 = pcl::PointXYZ; 41 | using PCLCloud3 = pcl::PointCloud; 42 | 43 | } // namespace calib 44 | } // namespace unipd 45 | 46 | #endif // UNIPD_CALIBRATION_CALIBRATON_PCL_BASE_DEFINITIONS_H_ 47 | -------------------------------------------------------------------------------- /calibration_pcl/include/calibration_pcl/base/pcl_eigen_conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_PCL_BASE_PCL_EIGEN_CONVERSIONS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_PCL_BASE_PCL_EIGEN_CONVERSIONS_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace unipd 37 | { 38 | namespace calib 39 | { 40 | 41 | template 42 | EigenT_ 43 | pcl2eigen (const pcl::PointCloud & in) 44 | { 45 | auto out = EigenT_(3, in.size()); 46 | for (size_t i = 0; i < in.size(); ++i) 47 | { 48 | const auto & p = in[i]; 49 | out.col(i) << p.x, p.y, p.z; 50 | } 51 | return out; 52 | } 53 | 54 | template 55 | EigenT_ 56 | pcl2eigen (const pcl::PointCloud & in, 57 | const std::vector & indices) 58 | { 59 | auto out = EigenT_(3, indices.size()); 60 | for (size_t i = 0; i < indices.size(); ++i) 61 | { 62 | const auto & p = in[indices[i]]; 63 | out.col(i) << p.x, p.y, p.z; 64 | } 65 | return out; 66 | } 67 | 68 | } // namespace calib 69 | } // namespace unipd 70 | #endif // UNIPD_CALIBRATION_CALIBRATION_PCL_BASE_PCL_EIGEN_CONVERSIONS_H_ 71 | -------------------------------------------------------------------------------- /calibration_pcl/include/calibration_pcl/depth/view.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATON_PCL_DEPTH_VIEW_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATON_PCL_DEPTH_VIEW_H_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace unipd 38 | { 39 | namespace calib 40 | { 41 | 42 | template 43 | class DepthViewPCL : public IndicesView_ 44 | { 45 | public: 46 | 47 | using Base = IndicesView_; 48 | // using Point = typename Base::Point; 49 | 50 | virtual 51 | ~DepthViewPCL () 52 | { 53 | // Do nothing 54 | } 55 | 56 | // protected: 57 | 58 | // inline virtual Point 59 | // computeCentroid () const 60 | // { 61 | // const auto & points = Base::indices(); 62 | // const auto & cloud = *Base::data(); 63 | // assert(not points.empty()); 64 | // Point sum = Point::Zero(); 65 | // for (Size1 i = 0; i < points.size(); ++i) 66 | // { 67 | // const PCLPoint3 & p = cloud[points[i]]; 68 | // sum.x() += p.x; 69 | // sum.y() += p.y; 70 | // sum.z() += p.z; 71 | // } 72 | // sum /= points.size(); 73 | // return sum; 74 | // } 75 | }; 76 | 77 | } // namespace calib 78 | } // namespace unipd 79 | 80 | #endif // UNIPD_CALIBRATION_CALIBRATON_PCL_DEPTH_VIEW_H_ 81 | -------------------------------------------------------------------------------- /calibration_pcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | calibration_pcl 5 | 1.0.0 6 | 7 | The calibration_pcl package 8 | 9 | Filippo Basso 10 | BSD 11 | 12 | 13 | 14 | catkin 15 | 16 | calibration_common 17 | 18 | boost 19 | 20 | libpcl-all-dev 21 | libpcl-all 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /calibration_pcl/test/base/test_pcl_eigen_conversions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | TEST(pcl_eigen, pcl2eigen_1) 33 | { 34 | pcl::PointCloud in(3, 2); // (width, height) 35 | in.at(0, 0) = pcl::PointXYZ{1, 2, 3}; 36 | in.at(0, 1) = pcl::PointXYZ{4, 5, 6}; 37 | in.at(1, 0) = pcl::PointXYZ{7, 8, 9}; 38 | in.at(1, 1) = pcl::PointXYZ{9, 8, 7}; 39 | in.at(2, 0) = pcl::PointXYZ{6, 0, 4}; 40 | in.at(2, 1) = pcl::PointXYZ{3, 2, 1}; 41 | 42 | auto out = unipd::calib::pcl2eigen>(in); 43 | 44 | for (int i = 0; i < 6; ++i) 45 | { 46 | EXPECT_EQ(in[i].x, out.col(i).x()); 47 | EXPECT_EQ(in[i].y, out.col(i).y()); 48 | EXPECT_EQ(in[i].z, out.col(i).z()); 49 | } 50 | } 51 | TEST(pcl_eigen, pcl2eigen_2) 52 | { 53 | pcl::PointCloud in(3, 2); // (width, height) 54 | in.at(0, 0) = pcl::PointXYZ{1, 2, 3}; 55 | in.at(0, 1) = pcl::PointXYZ{4, 5, 6}; 56 | in.at(1, 0) = pcl::PointXYZ{7, 8, 9}; 57 | in.at(1, 1) = pcl::PointXYZ{9, 8, 7}; 58 | in.at(2, 0) = pcl::PointXYZ{6, 0, 4}; 59 | in.at(2, 1) = pcl::PointXYZ{3, 2, 1}; 60 | 61 | std::vector indices{1, 2, 4}; 62 | 63 | auto out = unipd::calib::pcl2eigen>(in, indices); 64 | 65 | for (int i = 0; i < 3; ++i) 66 | { 67 | EXPECT_EQ(in[indices[i]].x, out.col(i).x()); 68 | EXPECT_EQ(in[indices[i]].y, out.col(i).y()); 69 | EXPECT_EQ(in[indices[i]].z, out.col(i).z()); 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /calibration_pcl/test/test_calibration_pcl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | 31 | int 32 | main(int argc, char ** argv) 33 | { 34 | ::testing::InitGoogleTest(&argc, argv); 35 | return RUN_ALL_TESTS(); 36 | } 37 | -------------------------------------------------------------------------------- /calibration_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibration_ros) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | 5 | ## Find catkin macros and libraries 6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 7 | ## is used, also find other catkin packages 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | image_transport 11 | cv_bridge 12 | calibration_pcl 13 | tf 14 | eigen_conversions 15 | ) 16 | #find_package(Eigen REQUIRED) 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED 19 | COMPONENTS 20 | common 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################### 32 | ## catkin specific configuration ## 33 | ################################### 34 | ## The catkin_package macro generates cmake config files for your package 35 | ## Declare things to be passed to dependent projects 36 | ## INCLUDE_DIRS: uncomment this if you package contains header files 37 | ## LIBRARIES: libraries you create in this project that dependent projects also need 38 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 39 | ## DEPENDS: system dependencies of this project that dependent projects also need 40 | catkin_package( 41 | INCLUDE_DIRS include 42 | LIBRARIES 43 | ${PROJECT_NAME}_sensors 44 | ${PROJECT_NAME}_devices 45 | ${PROJECT_NAME}_visualization 46 | CATKIN_DEPENDS 47 | image_transport 48 | cv_bridge 49 | calibration_pcl 50 | tf 51 | eigen_conversions 52 | # DEPENDS system_lib 53 | ) 54 | 55 | add_custom_target(${PROJECT_NAME}_HEADERS 56 | SOURCES 57 | include/${PROJECT_NAME}/sensors/ros_sensor.h 58 | include/${PROJECT_NAME}/sensors/pinhole_camera.h 59 | include/${PROJECT_NAME}/sensors/depth_sensor.h 60 | 61 | include/${PROJECT_NAME}/devices/ros_device.h 62 | 63 | include/${PROJECT_NAME}/visualization/objects.h 64 | ) 65 | 66 | ########### 67 | ## Build ## 68 | ########### 69 | 70 | ## Specify additional locations of header files 71 | ## Your package locations should be listed before other locations 72 | include_directories( 73 | include 74 | ${catkin_INCLUDE_DIRS} 75 | ${PCL_INCLUDE_DIRS} 76 | ) 77 | 78 | ## Declare a cpp library 79 | add_library(${PROJECT_NAME}_sensors 80 | src/${PROJECT_NAME}/sensors/pinhole_camera.cpp 81 | src/${PROJECT_NAME}/sensors/depth_sensor.cpp 82 | ) 83 | 84 | add_library(${PROJECT_NAME}_devices 85 | src/${PROJECT_NAME}/devices/ros_device.cpp 86 | ) 87 | 88 | add_library(${PROJECT_NAME}_visualization 89 | src/${PROJECT_NAME}/visualization/objects.cpp 90 | ) 91 | 92 | ## Declare a cpp executable 93 | # add_executable(calibration_ros_node src/calibration_ros_node.cpp) 94 | 95 | ## Add cmake target dependencies of the executable/library 96 | ## as an example, message headers may need to be generated before nodes 97 | # add_dependencies(calibration_ros_node calibration_ros_generate_messages_cpp) 98 | 99 | ## Specify libraries to link a library or executable target against 100 | target_link_libraries(${PROJECT_NAME}_sensors 101 | ${catkin_LIBRARIES} 102 | ) 103 | 104 | target_link_libraries(${PROJECT_NAME}_devices 105 | ${catkin_LIBRARIES} 106 | ${PROJECT_NAME}_sensors 107 | ) 108 | 109 | target_link_libraries(${PROJECT_NAME}_visualization 110 | ${catkin_LIBRARIES} 111 | ) 112 | 113 | ############# 114 | ## Install ## 115 | ############# 116 | 117 | # all install targets should use catkin DESTINATION variables 118 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 119 | 120 | ## Mark executable scripts (Python etc.) for installation 121 | ## in contrast to setup.py, you can choose the destination 122 | # install(PROGRAMS 123 | # scripts/my_python_script 124 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 125 | # ) 126 | 127 | ## Mark executables and/or libraries for installation 128 | # install(TARGETS calibration_ros calibration_ros_node 129 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 130 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 132 | # ) 133 | 134 | ## Mark cpp header files for installation 135 | # install(DIRECTORY include/${PROJECT_NAME}/ 136 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 137 | # FILES_MATCHING PATTERN "*.h" 138 | # PATTERN ".svn" EXCLUDE 139 | # ) 140 | 141 | ## Mark other files for installation (e.g. launch and bag files, etc.) 142 | # install(FILES 143 | # # myfile1 144 | # # myfile2 145 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 146 | # ) 147 | 148 | ############# 149 | ## Testing ## 150 | ############# 151 | 152 | ## Add gtest based cpp test target and link libraries 153 | 154 | ## Add folders to be run by python nosetests 155 | # catkin_add_nosetests(test) 156 | -------------------------------------------------------------------------------- /calibration_ros/include/calibration_ros/devices/ros_device.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ROS_DEVICES_ROS_DEVICE_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ROS_DEVICES_ROS_DEVICE_H_ 31 | 32 | #include 33 | #include 34 | 35 | namespace unipd 36 | { 37 | namespace calib 38 | { 39 | 40 | class ROSDevice 41 | { 42 | public: 43 | 44 | ROSDevice (ros::NodeHandle node_handle); 45 | 46 | bool 47 | allMessagesReceived () const 48 | { 49 | bool received = true; 50 | for (const std::shared_ptr & sensor : intensity_sensors_) 51 | { 52 | received = received and sensor->allMessagesReceived(); 53 | } 54 | for (const std::shared_ptr & sensor : depth_sensors_) 55 | { 56 | received = received and sensor->allMessagesReceived(); 57 | } 58 | return received; 59 | } 60 | 61 | bool 62 | waitForAllMessages (ros::Rate rate = ros::Rate(1.0)) const 63 | { 64 | while (ros::ok() and not allMessagesReceived()) 65 | { 66 | ros::spinOnce(); 67 | rate.sleep(); 68 | } 69 | return allMessagesReceived(); 70 | } 71 | 72 | std::vector> & 73 | intensitySensors () 74 | { 75 | return intensity_sensors_; 76 | } 77 | 78 | std::vector> & 79 | depthSensors () 80 | { 81 | return depth_sensors_; 82 | } 83 | 84 | private: 85 | 86 | std::string log_; 87 | 88 | std::vector> intensity_sensors_; 89 | std::vector> depth_sensors_; 90 | 91 | }; 92 | 93 | } // namespace calib 94 | } // namespace unipd 95 | #endif // UNIPD_CALIBRATION_CALIBRATION_ROS_DEVICES_ROS_DEVICE_H_ 96 | -------------------------------------------------------------------------------- /calibration_ros/include/calibration_ros/sensors/depth_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_DEPTH_SENSOR_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_DEPTH_SENSOR_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | namespace unipd 44 | { 45 | namespace calib 46 | { 47 | 48 | namespace ROSDepthSensorNS 49 | { 50 | 51 | enum Topic : int 52 | { 53 | NONE = 0, 54 | IMAGE = 1, 55 | CAMERA_INFO = 2, 56 | POINT_CLOUD = 4, 57 | ALL = IMAGE | CAMERA_INFO | POINT_CLOUD, 58 | }; 59 | 60 | struct Messages 61 | { 62 | sensor_msgs::Image::ConstPtr image_msg; 63 | sensor_msgs::CameraInfo::ConstPtr camera_info_msg; 64 | sensor_msgs::PointCloud2::ConstPtr cloud_msg; 65 | }; 66 | 67 | struct Data 68 | { 69 | cv::Mat image; 70 | PCLCloud3::Ptr cloud; 71 | }; 72 | 73 | } 74 | 75 | class ROSDepthSensor : public ROSSensor 76 | { 77 | public: 78 | 79 | using Topic = ROSDepthSensorNS::Topic; 80 | using Messages = ROSDepthSensorNS::Messages; 81 | using Data = ROSDepthSensorNS::Data; 82 | 83 | ROSDepthSensor (const ros::NodeHandle & node_handle); 84 | 85 | virtual 86 | ~ROSDepthSensor () {} 87 | 88 | virtual void 89 | subscribe (int topic); 90 | 91 | inline const std::shared_ptr & 92 | sensor () const 93 | { 94 | return sensor_; 95 | } 96 | 97 | inline bool 98 | isSensorSet () const 99 | { 100 | return sensor_->cameraModel().initialized(); 101 | } 102 | 103 | void 104 | imageCallback (const sensor_msgs::Image::ConstPtr & image_msg, 105 | const sensor_msgs::CameraInfo::ConstPtr & camera_info_msg); 106 | 107 | void 108 | cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr & camera_info_msg); 109 | 110 | void 111 | cloudCallback (const sensor_msgs::PointCloud2::ConstPtr & cloud_msg); 112 | 113 | virtual const Messages & 114 | lastMessages () const override 115 | { 116 | if (allMessagesReceived()) 117 | resetReceivedMessages(); 118 | return last_messages_; 119 | } 120 | 121 | virtual Data 122 | convertMessages (const Messages & messages) const override; 123 | 124 | protected: 125 | 126 | ros::NodeHandle node_handle_; 127 | image_transport::ImageTransport image_transport_; 128 | 129 | image_transport::CameraSubscriber image_sub_; 130 | ros::Subscriber camera_info_sub_; 131 | ros::Subscriber cloud_sub_; 132 | 133 | Messages last_messages_; 134 | 135 | std::shared_ptr sensor_; 136 | 137 | }; 138 | 139 | } // namespace calib 140 | } // namespace unipd 141 | #endif // UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_DEPTH_SENSOR_H_ 142 | -------------------------------------------------------------------------------- /calibration_ros/include/calibration_ros/sensors/pinhole_camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_UTILITIES_PINHOLE_CAMERA_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_PINHOLE_CAMERA_H_ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace unipd 40 | { 41 | namespace calib 42 | { 43 | 44 | namespace ROSPinholeCameraNS 45 | { 46 | 47 | enum Topic : int 48 | { 49 | NONE = 0, 50 | IMAGE = 1, 51 | CAMERA_INFO = 2, 52 | ALL = IMAGE | CAMERA_INFO, 53 | }; 54 | 55 | struct Messages 56 | { 57 | sensor_msgs::Image::ConstPtr image_msg; 58 | sensor_msgs::CameraInfo::ConstPtr camera_info_msg; 59 | }; 60 | 61 | struct Data 62 | { 63 | cv::Mat image; 64 | }; 65 | 66 | } 67 | 68 | class ROSPinholeCamera : public ROSSensor 69 | { 70 | public: 71 | 72 | using Topic = ROSPinholeCameraNS::Topic; 73 | using Messages = ROSPinholeCameraNS::Messages; 74 | using Data = ROSPinholeCameraNS::Data; 75 | 76 | // ROSPinholeCamera (const std::string & frame_id) 77 | // : ROSSensor(), 78 | // sensor_(make_shared()) 79 | // { 80 | // sensor_->setFrameId(frame_id); 81 | // } 82 | 83 | ROSPinholeCamera (const ros::NodeHandle & node_handle); 84 | 85 | virtual 86 | ~ROSPinholeCamera () {}; 87 | 88 | virtual void 89 | subscribe (int topic) override; 90 | 91 | inline const std::shared_ptr & 92 | sensor () const 93 | { 94 | return sensor_; 95 | } 96 | 97 | inline bool 98 | isSensorSet () const 99 | { 100 | return sensor_->cameraModel().initialized(); 101 | } 102 | 103 | void 104 | imageCallback (const sensor_msgs::Image::ConstPtr & image_msg, 105 | const sensor_msgs::CameraInfo::ConstPtr & camera_info_msg); 106 | 107 | virtual const Messages & 108 | lastMessages () const override 109 | { 110 | if (allMessagesReceived()) 111 | resetReceivedMessages(); 112 | return last_messages_; 113 | } 114 | 115 | virtual Data 116 | convertMessages (const Messages & messages) const override; 117 | 118 | protected: 119 | 120 | ros::NodeHandle node_handle_; 121 | image_transport::ImageTransport image_transport_; 122 | 123 | image_transport::CameraSubscriber image_sub_; 124 | 125 | Messages last_messages_; 126 | 127 | std::shared_ptr sensor_; 128 | 129 | }; 130 | 131 | } // namespace calib 132 | } // namespace unipd 133 | #endif // UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_PINHOLE_CAMERA_H_ 134 | -------------------------------------------------------------------------------- /calibration_ros/include/calibration_ros/sensors/ros_sensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_ROS_SENSOR_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_ROS_SENSOR_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | template 40 | class ROSSensor 41 | { 42 | public: 43 | 44 | using Topic = TopicT_; 45 | using Messages = MessagesT_; 46 | using Data = DataT_; 47 | 48 | ROSSensor () 49 | : topic_mask_(Topic::ALL), 50 | received_messages_(Topic::NONE) 51 | { 52 | // Do nothing 53 | } 54 | 55 | virtual 56 | ~ROSSensor () {} 57 | 58 | virtual void 59 | subscribe (int topic) = 0; 60 | 61 | bool 62 | allMessagesReceived () const 63 | { 64 | return topic_mask_ == received_messages_; 65 | } 66 | 67 | bool 68 | isMessagesReceived (int topic) const 69 | { 70 | return topic & received_messages_; 71 | } 72 | 73 | bool 74 | waitForMessage (int topic, 75 | ros::Rate rate = ros::Rate(1.0)) const 76 | { 77 | if (!(topic & topic_mask_)) 78 | return true; 79 | 80 | while (ros::ok() and not isMessagesReceived(topic)) 81 | { 82 | ros::spinOnce(); 83 | rate.sleep(); 84 | } 85 | return isMessagesReceived(topic); 86 | } 87 | 88 | bool 89 | waitForAllMessages (ros::Rate rate = ros::Rate(1.0)) const 90 | { 91 | while (ros::ok() and not allMessagesReceived()) 92 | { 93 | ros::spinOnce(); 94 | rate.sleep(); 95 | } 96 | return allMessagesReceived(); 97 | } 98 | 99 | virtual const Messages & 100 | lastMessages () const = 0; 101 | 102 | virtual Data 103 | convertMessages (const Messages & messages) const = 0; 104 | 105 | protected: 106 | 107 | void 108 | setTopicMask (int topic_mask) 109 | { 110 | topic_mask_ = topic_mask; 111 | } 112 | 113 | void 114 | newMessageReceived (int topic) const 115 | { 116 | received_messages_ |= topic; 117 | } 118 | 119 | void 120 | resetReceivedMessages () const 121 | { 122 | received_messages_ = Topic::NONE; 123 | } 124 | 125 | int topic_mask_; 126 | std::string log_; 127 | 128 | private: 129 | 130 | mutable int received_messages_; 131 | 132 | }; 133 | 134 | } // namespace calib 135 | } // namespace unipd 136 | #endif // UNIPD_CALIBRATION_CALIBRATION_ROS_SENSORS_ROS_SENSOR_H_ 137 | -------------------------------------------------------------------------------- /calibration_ros/include/calibration_ros/visualization/objects.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_CALIBRATION_ROS_VISUALIZATION_OBJECTS_H_ 30 | #define UNIPD_CALIBRATION_CALIBRATION_ROS_VISUALIZATION_OBJECTS_H_ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace unipd 38 | { 39 | namespace calib 40 | { 41 | 42 | geometry_msgs::TransformStamped 43 | toTransformStamped (const BaseObject & object); 44 | 45 | visualization_msgs::Marker 46 | toMarker (const BaseObject & object); 47 | 48 | visualization_msgs::Marker 49 | toMarker (const PlanarObject & object); 50 | 51 | visualization_msgs::Marker 52 | toMarker (const Checkerboard & checkerboard); 53 | 54 | 55 | } // namespace calib 56 | } // namespace unipd 57 | #endif // UNIPD_CALIBRATION_CALIBRATION_ROS_VISUALIZATION_OBJECTS_H_ 58 | -------------------------------------------------------------------------------- /calibration_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | calibration_ros 4 | 1.0.0 5 | The calibration_ros package 6 | 7 | 8 | 9 | 10 | Filippo Basso 11 | 12 | 13 | 14 | 15 | 16 | BSD 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 | 44 | calibration_pcl 45 | image_transport 46 | cv_bridge 47 | tf 48 | eigen_conversions 49 | 50 | calibration_pcl 51 | image_transport 52 | cv_bridge 53 | tf 54 | eigen_conversions 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /calibration_ros/src/calibration_ros/devices/ros_device.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | 31 | namespace unipd 32 | { 33 | namespace calib 34 | { 35 | 36 | ROSDevice::ROSDevice (ros::NodeHandle node_handle) 37 | { 38 | std::string name; 39 | node_handle.param("name", name, std::string("device")); 40 | log_ = "[" + name + "] "; 41 | 42 | if (not node_handle.hasParam("sensors")) 43 | ROS_FATAL_STREAM(log_ << "Missing \"sensors\" parameter!"); 44 | 45 | std::map> sensor_map; 46 | 47 | std::vector intensity_sensors; 48 | node_handle.getParam("sensors/intensity", intensity_sensors); 49 | for (const std::string & sensor_name : intensity_sensors) 50 | { 51 | std::shared_ptr sensor = std::make_shared(ros::NodeHandle(node_handle, sensor_name)); 52 | intensity_sensors_.push_back(sensor); 53 | sensor_map[sensor_name] = sensor->sensor(); 54 | } 55 | 56 | std::vector depth_sensors; 57 | node_handle.getParam("sensors/depth", depth_sensors); 58 | for (const std::string & sensor_name : depth_sensors) 59 | { 60 | std::shared_ptr sensor = std::make_shared(ros::NodeHandle(node_handle, sensor_name)); 61 | depth_sensors_.push_back(sensor); 62 | sensor_map[sensor_name] = sensor->sensor(); 63 | } 64 | 65 | std::vector sensors = intensity_sensors; 66 | sensors.insert(sensors.end(), depth_sensors.begin(), depth_sensors.end()); 67 | if (node_handle.hasParam("transforms")) 68 | { 69 | for (const std::string & sensor_name : sensors) 70 | { 71 | if (node_handle.hasParam("transforms/" + sensor_name)) 72 | { 73 | std::string parent; 74 | node_handle.getParam("transforms/" + sensor_name + "/parent", parent); 75 | 76 | if (sensor_map.find(parent) == sensor_map.end()) 77 | ROS_FATAL_STREAM(log_ << "Unknown sensor \"" << parent<< "\" in \"transforms\"!"); 78 | 79 | std::map translation; 80 | std::map rotation; 81 | node_handle.getParam("transforms/" + sensor_name + "/translation", translation); 82 | node_handle.getParam("transforms/" + sensor_name + "/rotation", rotation); 83 | 84 | Translation3 t = Translation3(translation["x"], translation["y"], translation["z"]); 85 | Quaternion q = Quaternion(rotation["w"], rotation["x"], rotation["y"], rotation["z"]); 86 | 87 | sensor_map[sensor_name]->setParent(sensor_map[parent]); 88 | sensor_map[sensor_name]->setPose(t * q); 89 | } 90 | } 91 | } 92 | } 93 | 94 | } // namespace calib 95 | } // namespace unipd 96 | -------------------------------------------------------------------------------- /calibration_ros/src/calibration_ros/sensors/pinhole_camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #include 30 | #include 31 | 32 | namespace unipd 33 | { 34 | namespace calib 35 | { 36 | 37 | ROSPinholeCamera::ROSPinholeCamera (const ros::NodeHandle & node_handle) 38 | : ROSSensor(), 39 | node_handle_(node_handle), 40 | image_transport_(node_handle), 41 | sensor_(std::make_shared()) 42 | { 43 | std::string frame_id; 44 | node_handle_.param("frame_id", frame_id, std::string("camera")); 45 | sensor_->setFrameId(frame_id); 46 | log_ = "[" + frame_id + "] "; 47 | } 48 | 49 | void 50 | ROSPinholeCamera::subscribe (int topic) 51 | { 52 | assert (static_cast(topic & Topic::IMAGE) == static_cast(topic & Topic::CAMERA_INFO)); 53 | if (topic & Topic::IMAGE) 54 | { 55 | image_sub_ = image_transport_.subscribeCamera("image", 1, &ROSPinholeCamera::imageCallback, this); 56 | ROS_DEBUG_STREAM(log_ << "Subscribed to \"" << image_sub_.getTopic() << "\" topic."); 57 | ROS_DEBUG_STREAM(log_ << "Subscribed to \"" << image_sub_.getInfoTopic() << "\" topic."); 58 | } 59 | else 60 | { 61 | ROS_DEBUG_STREAM(log_ << "No subscription."); 62 | } 63 | setTopicMask(topic); 64 | } 65 | 66 | void 67 | ROSPinholeCamera::imageCallback (const sensor_msgs::Image::ConstPtr & image_msg, 68 | const sensor_msgs::CameraInfo::ConstPtr & camera_info_msg) 69 | { 70 | last_messages_.image_msg = image_msg; 71 | last_messages_.camera_info_msg = camera_info_msg; 72 | 73 | if (not isSensorSet()) 74 | { 75 | sensor_msgs::CameraInfo new_camera_info_msg = *camera_info_msg; 76 | new_camera_info_msg.header.frame_id = sensor_->frameId(); 77 | sensor_->setCameraModel(PinholeCameraModel(new_camera_info_msg)); 78 | } 79 | 80 | newMessageReceived(Topic::IMAGE | Topic::CAMERA_INFO); 81 | } 82 | 83 | auto 84 | ROSPinholeCamera::convertMessages (const Messages & messages) const -> Data 85 | { 86 | cv::Mat image; 87 | auto image_ptr = cv_bridge::toCvCopy(messages.image_msg); 88 | return Data{image_ptr->image}; 89 | } 90 | 91 | } // namespace calib 92 | } // namespace unipd 93 | -------------------------------------------------------------------------------- /calibration_toolkit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibration_toolkit) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /calibration_toolkit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | calibration_toolkit 4 | 1.0.0 5 | The calibration_toolkit package 6 | Filippo Basso 7 | BSD 8 | catkin 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /calibration_toolkit/scripts/install_ceres.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Ceres-solver installation following instructions at http://ceres-solver.org/building.html 3 | 4 | # Install dependencies 5 | sudo add-apt-repository ppa:bzindovic/suitesparse-bugfix-1319687 -y 6 | sudo apt-get update 7 | sudo apt-get install libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev -y --force-yes 8 | 9 | # Create build directory 10 | mkdir /tmp/ceres_install 11 | cd /tmp/ceres_install 12 | 13 | # Download latest tested version: commit 5a21b8b1e9f0535c4b0710853bdb7bec42d5febc 14 | git clone https://ceres-solver.googlesource.com/ceres-solver 15 | cd ceres-solver 16 | git checkout 5a21b8b1e9f0535c4b0710853bdb7bec42d5febc 17 | cd .. 18 | 19 | # Build as a shared library 20 | mkdir ceres-bin 21 | cd ceres-bin 22 | cmake -DBUILD_SHARED_LIBS=ON ../ceres-solver 23 | make -j8 && make test 24 | 25 | # Install 26 | sudo make install 27 | 28 | # Clean 29 | sudo rm -r /tmp/ceres_install 30 | -------------------------------------------------------------------------------- /multisensor_calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(multisensor_calibration) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 4 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O1") 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED 10 | COMPONENTS 11 | calibration_ros 12 | calibration_msgs 13 | calibration_algorithms 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | find_package(Ceres 1.10 REQUIRED) 18 | 19 | set(yaml-cpp_INCLUDE_DIRS /usr/include/yaml-cpp/) 20 | set(yaml-cpp_LIBRARIES yaml-cpp) 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################### 28 | ## catkin specific configuration ## 29 | ################################### 30 | ## The catkin_package macro generates cmake config files for your package 31 | ## Declare things to be passed to dependent projects 32 | ## INCLUDE_DIRS: uncomment this if you package contains header files 33 | ## LIBRARIES: libraries you create in this project that dependent projects also need 34 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 35 | ## DEPENDS: system dependencies of this project that dependent projects also need 36 | catkin_package( 37 | INCLUDE_DIRS 38 | include 39 | # LIBRARIES ${PROJECT_NAME} 40 | CATKIN_DEPENDS 41 | calibration_ros 42 | calibration_msgs 43 | calibration_algorithms 44 | DEPENDS 45 | yaml-cpp 46 | ) 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | add_custom_target(${PROJECT_NAME}_HEADERS 53 | SOURCES 54 | include/${PROJECT_NAME}/common.h 55 | include/${PROJECT_NAME}/device_node.h 56 | include/${PROJECT_NAME}/master_node.h 57 | include/${PROJECT_NAME}/calibration.h 58 | include/${PROJECT_NAME}/optimization.h 59 | 60 | include/${PROJECT_NAME}/corner_based/errors.h 61 | include/${PROJECT_NAME}/corner_based/plane_errors.h 62 | ) 63 | 64 | ## Specify additional locations of header files 65 | ## Your package locations should be listed before other locations 66 | include_directories( 67 | include 68 | ${catkin_INCLUDE_DIRS} 69 | ${yaml-cpp_INCLUDE_DIRS} 70 | ${CERES_INCLUDE_DIRS} 71 | ) 72 | 73 | ## Declare a cpp library 74 | #add_library(${PROJECT_NAME} 75 | #) 76 | 77 | ## Declare a cpp executable 78 | add_executable(master_node 79 | src/${PROJECT_NAME}/master_node.cpp 80 | src/${PROJECT_NAME}/calibration.cpp 81 | src/${PROJECT_NAME}/optimization.cpp 82 | ) 83 | 84 | add_executable(device_node 85 | src/${PROJECT_NAME}/device_node.cpp 86 | ) 87 | 88 | #add_executable(pinhole_rgb_camera_node 89 | # src/${PROJECT_NAME}/devices/pinhole_rgb_camera.cpp 90 | # src/${PROJECT_NAME}/pinhole_rgb_camera_node.cpp 91 | #) 92 | 93 | ## Add cmake target dependencies of the executable/library 94 | ## as an example, message headers may need to be generated before nodes 95 | add_dependencies(master_node calibration_msgs) 96 | add_dependencies(device_node calibration_msgs) 97 | #add_dependencies(pinhole_rgb_camera_node calibration_msgs) 98 | 99 | ## Specify libraries to link a library or executable target against 100 | #target_link_libraries(${PROJECT_NAME} 101 | # ${catkin_LIBRARIES} 102 | #) 103 | 104 | target_link_libraries(master_node 105 | ${catkin_LIBRARIES} 106 | ${yaml-cpp_LIBRARIES} 107 | ${CERES_LIBRARIES} 108 | ) 109 | 110 | target_link_libraries(device_node 111 | ${catkin_LIBRARIES} 112 | ) 113 | 114 | #target_link_libraries(pinhole_rgb_camera_node 115 | # ${PROJECT_NAME} 116 | # ${catkin_LIBRARIES} 117 | #) 118 | 119 | ############# 120 | ## Install ## 121 | ############# 122 | 123 | # all install targets should use catkin DESTINATION variables 124 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 125 | 126 | ## Mark executable scripts (Python etc.) for installation 127 | ## in contrast to setup.py, you can choose the destination 128 | # install(PROGRAMS 129 | # scripts/my_python_script 130 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 131 | # ) 132 | 133 | ## Mark executables and/or libraries for installation 134 | # install(TARGETS multisensor_calibration multisensor_calibration_node 135 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 138 | # ) 139 | 140 | ## Mark cpp header files for installation 141 | # install(DIRECTORY include/${PROJECT_NAME}/ 142 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 143 | # FILES_MATCHING PATTERN "*.h" 144 | # PATTERN ".svn" EXCLUDE 145 | # ) 146 | 147 | ## Mark other files for installation (e.g. launch and bag files, etc.) 148 | # install(FILES 149 | # # myfile1 150 | # # myfile2 151 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 152 | # ) 153 | 154 | ############# 155 | ## Testing ## 156 | ############# 157 | 158 | ## Add gtest based cpp test target and link libraries 159 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_multisensor_calibration.cpp) 160 | # if(TARGET ${PROJECT_NAME}-test) 161 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 162 | # endif() 163 | 164 | ## Add folders to be run by python nosetests 165 | # catkin_add_nosetests(test) 166 | 167 | -------------------------------------------------------------------------------- /multisensor_calibration/conf/checkerboard.yaml: -------------------------------------------------------------------------------- 1 | # Checkerboard configuration 2 | checkerboard : 3 | cols : 6 4 | rows : 5 5 | cell_width : 0.12 6 | cell_height : 0.12 7 | -------------------------------------------------------------------------------- /multisensor_calibration/conf/network.yaml: -------------------------------------------------------------------------------- 1 | # Camera network configuration 2 | 3 | network: 4 | - pc: "Phoenix" 5 | sensors: ["camera"] 6 | - pc: "Gemini" 7 | sensors: ["camera"] 8 | -------------------------------------------------------------------------------- /multisensor_calibration/conf/pointgrey_camera_default.yaml: -------------------------------------------------------------------------------- 1 | video_mode: format7_mode0 2 | format7_color_coding: raw8 3 | 4 | auto_packet_size: false 5 | packet_size: 1400 6 | packet_delay: 4000 7 | 8 | frame_rate: 30.0 9 | 10 | auto_exposure: true 11 | exposure: 0.0 12 | 13 | auto_shutter: true 14 | shutter_speed: 0.0 15 | 16 | auto_gain: true 17 | gain: 0.0 18 | 19 | brightness: 0.0 20 | gamma: 1.0 21 | white_balance_blue: 900 22 | white_balance_red: 550 23 | -------------------------------------------------------------------------------- /multisensor_calibration/include/multisensor_calibration/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_COMMON_H_ 30 | #define UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_COMMON_H_ 31 | 32 | #include 33 | 34 | namespace unipd 35 | { 36 | namespace calib 37 | { 38 | 39 | enum class SensorType 40 | { 41 | INTENSITY, 42 | DEPTH 43 | }; 44 | 45 | struct Data {}; 46 | 47 | struct IntensityData : public Data 48 | { 49 | IntensityData (const Cloud2 & corners) : corners(corners) {} 50 | IntensityData (Cloud2 && corners) : corners(corners) {} 51 | Cloud2 corners; 52 | }; 53 | 54 | struct DepthData : public Data 55 | { 56 | DepthData (const Plane3 & plane) : plane(plane) {} 57 | DepthData (Plane3 && plane) : plane(plane) {} 58 | Plane3 plane; 59 | }; 60 | 61 | } // namespace calib 62 | } // namespace unipd 63 | #endif // UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_COMMON_H_ 64 | -------------------------------------------------------------------------------- /multisensor_calibration/include/multisensor_calibration/device_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_KINECT1_NODE_H_ 30 | #define UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_KINECT1_NODE_H_ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace unipd 44 | { 45 | namespace calib 46 | { 47 | 48 | class DeviceNode 49 | { 50 | public: 51 | 52 | DeviceNode (const ros::NodeHandle & node_handle); 53 | 54 | bool 55 | getDeviceInfo (calibration_msgs::GetDeviceInfo::Request & request, 56 | calibration_msgs::GetDeviceInfo::Response & response); 57 | 58 | bool 59 | initialize (); 60 | 61 | void 62 | extractCheckerboard (const calibration_msgs::CheckerboardExtractionGoal::ConstPtr & goal); 63 | 64 | private: 65 | 66 | CheckerboardExtraction::ImageResult 67 | extractCheckerboardFromSensor (const std::shared_ptr & sensor, 68 | const Checkerboard & checkerboard, 69 | int extraction_number); 70 | 71 | CheckerboardExtraction::DepthResult 72 | extractCheckerboardFromSensor (const std::shared_ptr & sensor, 73 | const CheckerboardExtraction::ImageResult & image_result, 74 | int extraction_number); 75 | 76 | ros::NodeHandle node_handle_; 77 | ros::ServiceServer get_sensor_info_srv_; 78 | 79 | using CheckerboardExtractionServer = actionlib::SimpleActionServer; 80 | CheckerboardExtractionServer cb_extraction_server_; 81 | 82 | ROSDevice device_; 83 | 84 | std::string log_; 85 | 86 | }; 87 | 88 | } // namespace calib 89 | } // namespace unipd 90 | #endif // UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_KINECT1_NODE_H_ 91 | -------------------------------------------------------------------------------- /multisensor_calibration/include/multisensor_calibration/master_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_CALIBRATION_MASTER_NODE_H_ 30 | #define UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_CALIBRATION_MASTER_NODE_H_ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | //#include 48 | 49 | #include 50 | 51 | using namespace actionlib; 52 | 53 | namespace unipd 54 | { 55 | namespace calib 56 | { 57 | 58 | using CheckerboardExtractionClient = SimpleActionClient; 59 | 60 | class CalibrationMasterNode 61 | { 62 | public: 63 | 64 | struct Device 65 | { 66 | std::string name; 67 | ros::ServiceClient get_device_info_client; 68 | std::shared_ptr cb_extraction_client; 69 | }; 70 | 71 | struct PC 72 | { 73 | std::string name; 74 | std::vector device_vec; 75 | }; 76 | 77 | CalibrationMasterNode (const ros::NodeHandle & node_handle); 78 | 79 | void 80 | actionCallback (const std_msgs::String::ConstPtr & msg); 81 | 82 | void 83 | acquisitionCallback (const std_msgs::Empty::ConstPtr & msg); 84 | 85 | bool 86 | getResultsCallback (calibration_msgs::GetCalibrationResults::Request & request, 87 | calibration_msgs::GetCalibrationResults::Response & response); 88 | 89 | bool 90 | initialize (); 91 | 92 | bool 93 | callGetDeviceInfo (Device & device, 94 | std::vector> & pinhole_sensors, 95 | std::vector> & depth_sensors); 96 | 97 | void 98 | performAcquisition (); 99 | 100 | void 101 | spinOnce (); 102 | 103 | void 104 | spin (); 105 | 106 | private: 107 | 108 | bool 109 | parseNetworkFile (const std::string & filename); 110 | 111 | template 112 | void 113 | publish (const ObjectT_ & object, 114 | const std::string & ns, 115 | int id) 116 | { 117 | if (not object.hasParent()) 118 | return; 119 | auto marker = toMarker(object); 120 | marker.ns = ns; 121 | marker.id = id; 122 | marker_pub_.publish(marker); 123 | tf_pub_.sendTransform(toTransformStamped(object)); 124 | } 125 | 126 | ros::NodeHandle node_handle_; 127 | 128 | std::string log_; 129 | 130 | ros::Subscriber acquisition_sub_; 131 | ros::Subscriber action_sub_; 132 | ros::Publisher marker_pub_; 133 | ros::ServiceServer result_service_; 134 | tf::TransformBroadcaster tf_pub_; 135 | 136 | std::shared_ptr checkerboard_; 137 | std::vector pcs_; 138 | 139 | enum : int {SENSORS = 0, CHECKERBOARDS = 1, PLANES = 2, N = 3}; 140 | std::vector frame_ids_[N]; 141 | 142 | Calibration calibration_; 143 | bool plane_flag_; 144 | }; 145 | 146 | } // namespace calib 147 | } // namespace unipd 148 | #endif // UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_CALIBRATION_MASTER_NODE_H_ 149 | -------------------------------------------------------------------------------- /multisensor_calibration/include/multisensor_calibration/optimization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015-, Filippo Basso 3 | * 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * 3. Neither the name of the copyright holder(s) nor the 14 | * names of its contributors may be used to endorse or promote products 15 | * derived from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | * ANY 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 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 | 29 | #ifndef UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_OPTIMIZATION_H_ 30 | #define UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_OPTIMIZATION_H_ 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | namespace unipd 43 | { 44 | namespace calib 45 | { 46 | 47 | template 48 | struct RawTransform_ 49 | { 50 | RawTransform_ (const Pose3 & pose) 51 | { 52 | Pose3Parser parser; 53 | parser.fromPose3(pose, data); 54 | } 55 | 56 | Scalar data[R_ + t_]; 57 | }; 58 | 59 | class Optimization 60 | { 61 | public: 62 | 63 | using RawTransform = RawTransform_<4, 3>; 64 | using IntensityError = IntensityError_<4, 3>; 65 | using DepthError = DepthError_<4, 3>; 66 | using OnPlaneIntensityError = OnPlaneIntensityError_<4, 3>; 67 | using OnPlaneDepthError = OnPlaneDepthError_<4, 3>; 68 | using PlaneParameterization = PlaneParameterization_<4, 3>; 69 | using Pose2DParameterization = Pose2DParameterization_<4, 3>; 70 | using Pose3DParameterization = Pose3DParameterization_<4, 3>; 71 | 72 | void 73 | addObject (const std::shared_ptr & object); 74 | 75 | void 76 | setVariable (const std::shared_ptr & object); 77 | 78 | void 79 | setFixed (const std::shared_ptr & object); 80 | 81 | void 82 | set6DOFTransform (const std::shared_ptr & object); 83 | 84 | void 85 | setPlaneTransform (const std::shared_ptr & object); 86 | 87 | void 88 | set3DOFTransform (const std::shared_ptr & object); 89 | 90 | void 91 | addConstraint (const std::shared_ptr & checkerboard, 92 | const std::shared_ptr & sensor, 93 | const std::shared_ptr & data); 94 | 95 | void 96 | addConstraint (const std::shared_ptr & checkerboard, 97 | const std::shared_ptr & sensor, 98 | const std::shared_ptr & data); 99 | 100 | void 101 | addConstraint (const std::shared_ptr & checkerboard, 102 | const std::shared_ptr & plane, 103 | const std::shared_ptr & sensor, 104 | const std::shared_ptr & data); 105 | 106 | void 107 | addConstraint (const std::shared_ptr & checkerboard, 108 | const std::shared_ptr & plane, 109 | const std::shared_ptr & sensor, 110 | const std::shared_ptr & data); 111 | 112 | void 113 | perform (); 114 | 115 | Transform3 116 | estimatedTransform (const std::shared_ptr & object) const; 117 | 118 | void 119 | reset (); 120 | 121 | private: 122 | 123 | std::string log_ = "[/optimization] "; 124 | 125 | std::map, std::shared_ptr> transform_map_; 126 | 127 | ceres::Problem problem_; 128 | Pose3Parser<4, 3> parser_; 129 | 130 | }; 131 | 132 | } // namespace calib 133 | } // namespace unipd 134 | 135 | #endif // UNIPD_CALIBRATION_MULTISENSOR_CALIBRATION_OPTIMIZATION_H_ 136 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/camera_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | name: "$(arg camera_name)" 20 | sensors: 21 | intensity: [rgb] 22 | depth: [] 23 | rgb: 24 | frame_id: "/$(arg pc_name)/$(arg camera_name)/rgb" 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/kinect1_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | name: "$(arg kinect_name)" 30 | sensors: 31 | intensity: [rgb] 32 | depth: [depth] 33 | rgb: 34 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/rgb" 35 | depth: 36 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/depth" 37 | error: 38 | max_degree: 2 39 | min_degree: 2 40 | coefficients: [0.0035] 41 | transforms: 42 | depth: 43 | parent: rgb 44 | translation: {x: -0.025, y: 0.0, z: 0.0} 45 | rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} 46 | 47 | 48 | 49 | name: "$(arg kinect_name)" 50 | sensors: 51 | intensity: [rgb] 52 | depth: [] 53 | rgb: 54 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/rgb" 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/kinect2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | name: "$(arg kinect_name)" 27 | sensors: 28 | intensity: [rgb] 29 | depth: [depth] 30 | rgb: 31 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/rgb" 32 | depth: 33 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/depth" 34 | image_type: uint16 35 | error: 36 | max_degree: 0 37 | min_degree: 0 38 | coefficients: [0.02] 39 | transforms: 40 | depth: 41 | parent: rgb 42 | position: {x: 0.052, y: 0.0, z: 0.0} 43 | orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/kinect2_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/kinect2_rgb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | name: "$(arg kinect_name)" 27 | sensors: 28 | intensity: [rgb] 29 | depth: [] 30 | rgb: 31 | frame_id: "/$(arg pc_name)/$(arg kinect_name)/rgb" 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/master_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/pg_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /multisensor_calibration/launch/pg_camera_old.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | name: "$(arg camera_name)" 37 | sensors: 38 | intensity: [rgb] 39 | rgb: 40 | frame_id: "/$(arg pc_name)/$(arg camera_name)/rgb" 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /multisensor_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | multisensor_calibration 5 | 1.0.0 6 | The multisensor_calibration package 7 | Filippo Basso 8 | BSD 9 | 10 | 11 | 12 | catkin 13 | 14 | calibration_ros 15 | calibration_msgs 16 | calibration_algorithms 17 | 18 | libyaml-cpp-dev 19 | 20 | 21 | 22 | 23 | 24 | --------------------------------------------------------------------------------