[ { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(rgbd_calibration)\nset(CMAKE_BUILD_TYPE RelWithDebInfo)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS cmake_modules roscpp calibration_common geometry_msgs kinect\n eigen_conversions camera_info_manager cv_bridge pcl_ros\n image_transport)# swissranger_camera)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\nfind_package(Boost REQUIRED)\nfind_package(Eigen REQUIRED)\nfind_package(PCL 1.7 REQUIRED)\nfind_package(OpenCV REQUIRED)\nfind_package(OpenMP REQUIRED)\nfind_package(Ceres REQUIRED)\n\nset(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}\")\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}\")\nset(CMAKE_EXE_LINKER_FLAGS \"${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}\")\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and scripts declared therein get installed\n# catkin_python_setup()\n\n#######################################\n## Declare ROS messages and services ##\n#######################################\n\n## Generate messages in the 'msg' folder\nadd_message_files(\n FILES\n Acquisition.msg\n)\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate added messages and services with any dependencies listed here\ngenerate_messages(\n DEPENDENCIES\n std_msgs\n)\n\n###################################################\n## Declare things to be passed to other projects ##\n###################################################\n\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n INCLUDE_DIRS include\n LIBRARIES interactive_checkerboard_finder rgbd_calibration\n CATKIN_DEPENDS roscpp calibration_common geometry_msgs kinect\n eigen_conversions camera_info_manager cv_bridge pcl_ros image_transport\n DEPENDS eigen pcl opencv2\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\ninclude_directories(include\n ${catkin_INCLUDE_DIRS}\n ${EIGEN_INCLUDE_DIRS}\n ${PCL_INCLUDE_DIRS}\n ${CERES_INCLUDE_DIRS}\n)\n\n## Declare a cpp library\n\nadd_library(rgbd_calibration\n src/rgbd_calibration/calibration.cpp include/rgbd_calibration/calibration.h\n include/rgbd_calibration/globals.h\n src/rgbd_calibration/depth_undistortion_estimation.cpp include/rgbd_calibration/depth_undistortion_estimation.h\n src/rgbd_calibration/checkerboard_views.cpp include/rgbd_calibration/checkerboard_views.h\n src/rgbd_calibration/checkerboard_views_extractor.cpp include/rgbd_calibration/checkerboard_views_extractor.h\n src/rgbd_calibration/publisher.cpp include/rgbd_calibration/publisher.h\n)\n\nadd_executable(rgbd_offline_calibration\n src/rgbd_calibration/calibration_node.cpp include/rgbd_calibration/calibration_node.h\n src/rgbd_calibration/offline_calibration_node.cpp include/rgbd_calibration/offline_calibration_node.h\n)\n\n#add_executable(simulation\n# src/rgbd_calibration/simulation_node.cpp include/rgbd_calibration/simulation_node.h\n#)\n\nadd_executable(test_calibration\n src/rgbd_calibration/test_node.cpp include/rgbd_calibration/test_node.h\n src/rgbd_calibration/calibration_test.cpp include/rgbd_calibration/calibration_test.h\n)\n\nadd_executable(data_collection\n src/rgbd_calibration/data_collection_node.cpp\n)\n\n## Add dependencies to the executable\n# add_dependencies(calibration_node ${PROJECT_NAME})\n\n## Specify libraries to link a library or executable target against\n\ntarget_link_libraries(rgbd_calibration\n ${catkin_LIBRARIES}\n ${PCL_LIBRARIES}\n ${OpenCV_LIBS}\n ${CERES_LIBRARIES}\n)\n\ntarget_link_libraries(rgbd_offline_calibration\n rgbd_calibration\n ${catkin_LIBRARIES}\n ${PCL_LIBRARIES}\n ${OpenCV_LIBS}\n ${CERES_LIBRARIES}\n)\n\n#target_link_libraries(simulation\n# rgbd_calibration\n# ${catkin_LIBRARIES}\n# ${PCL_LIBRARIES}\n# ${OpenCV_LIBS}\n# ${CERES_LIBRARIES}\n#)\n\ntarget_link_libraries(test_calibration\n rgbd_calibration\n ${catkin_LIBRARIES}\n ${PCL_LIBRARIES}\n ${OpenCV_LIBS}\n ${CERES_LIBRARIES}\n)\n\ntarget_link_libraries(data_collection\n ${catkin_LIBRARIES}\n ${PCL_LIBRARIES}\n ${OpenCV_LIBS}\n)\n\n#############\n## Install ##\n#############\n\n## Mark executable scripts (Python etc.) for installation\n## not required for python when using catkin_python_setup()\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables and/or libraries for installation\n# install(TARGETS calibration calibration_node\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/polynomial_undistortion_matrix_multifit_test.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test \n# ${catkin_LIBRARIES}\n# ${PCL_LIBRARIES}\n# ${OpenCV_LIBS}\n# ${CERES_LIBRARIES})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "README.md", "content": "## Dependencies\n\nThis package depends on [calibration_toolkit](https://github.com/iaslab-unipd/calibration_toolkit) version 0.3.2 (i.e. `git checkout v0.3.2`).\n\n## Initial Setup\n\nEdit **conf/checkerboards.yaml** to match your checkerboard(s) parameters.\n\n## Data Collection\n\nUse the [sensor_data_collection](https://github.com/iaslab-unipd/sensor_data_collection) package for collecting data from your sensors.\n\n## Calibration\n\nFile **launch/kinect_47A_tro.launch** is a sample file to run the offline calibration of a Kinect 1.\nModify it to match your setup and perform the calibration.\n\n" }, { "path": "conf/checkerboards.yaml", "content": "checkerboards:\n#- rows: 7\n# cols: 6\n# cell_width: 0.095\n# cell_height: 0.096\n- rows: 5\n cols: 6\n cell_width: 0.12\n cell_height: 0.12\n#- rows: 10\n# cols: 9\n# cell_width: 0.058\n# cell_height: 0.0583\n#- rows: 5\n# cols: 8\n# cell_width: 0.030\n# cell_height: 0.030\n" }, { "path": "conf/checkerboards_pepper.yaml", "content": "checkerboards:\n- rows: 7\n cols: 9\n cell_width: 0.02\n cell_height: 0.02\n" }, { "path": "conf/sim_camera.yaml", "content": "image_width: 640\nimage_height: 480\ncamera_name: sim_camera\ncamera_matrix:\n rows: 3\n cols: 3\n data: [ 525, 0, 320, 0, 525, 240, 0, 0, 1 ]\ndistortion_model: plumb_bob\ndistortion_coefficients:\n rows: 1\n cols: 5\n data: [ 0, 0, 0, 0, 0 ]\nrectification_matrix:\n rows: 3\n cols: 3\n data: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ]\nprojection_matrix:\n rows: 3\n cols: 4\n data: [ 525, 0, 320, 0, 0, 525, 240, 0, 0, 0, 1, 0 ]" }, { "path": "include/rgbd_calibration/calibration.h", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef RGBD_CALIBRATION_CALIBRATION_H_\n#define RGBD_CALIBRATION_CALIBRATION_H_\n\n#include \n\n#include \n\n#include \n#include \n#include \n#include \n\nnamespace calibration\n{\n\nclass Calibration\n{\npublic:\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n inline void\n setColorSensor (const PinholeSensor::Ptr & color_sensor)\n {\n color_sensor_ = color_sensor;\n }\n\n inline void\n setDepthSensor (const KinectDepthSensor::Ptr & depth_sensor)\n {\n depth_sensor_ = depth_sensor;\n\n depth_intrinsics_.resize(4);\n depth_intrinsics_[0] = depth_sensor->cameraModel()->intrinsicMatrix()(0, 0);\n depth_intrinsics_[1] = depth_sensor->cameraModel()->intrinsicMatrix()(1, 1);\n depth_intrinsics_[2] = depth_sensor->cameraModel()->intrinsicMatrix()(0, 2);\n depth_intrinsics_[3] = depth_sensor->cameraModel()->intrinsicMatrix()(1, 2);\n }\n\n const std::vector &\n optimizedIntrinsics () const\n {\n\treturn depth_intrinsics_;\n }\n\n inline void\n setCheckerboards (const std::vector & cb_vec)\n {\n cb_vec_ = cb_vec;\n }\n\n inline void\n setPublisher (const Publisher::Ptr & publisher)\n {\n publisher_ = publisher;\n }\n\n inline void\n setDownSampleRatio (int ratio)\n {\n assert(ratio > 0);\n ratio_ = ratio;\n }\n\n void\n addData (const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud);\n\n// void\n// addTestData (const cv::Mat & image,\n// const PCLCloud3::ConstPtr & cloud);\n\n inline void\n setEstimateInitialTransform (bool estimate_initial_trasform)\n {\n estimate_initial_trasform_ = estimate_initial_trasform;\n }\n\n void\n initDepthUndistortionModel ()\n {\n assert(local_matrix_ and global_matrix_);\n estimate_depth_und_model_ = true;\n\n depth_undistortion_estimation_ = boost::make_shared();\n depth_undistortion_estimation_->setDepthErrorFunction(depth_sensor_->depthErrorFunction());\n //depth_undistortion_estimation_->setDepthErrorFunction(Polynomial(Vector3(0.000, 0.000, 0.0035)));\n depth_undistortion_estimation_->setLocalModel(local_model_);\n depth_undistortion_estimation_->setGlobalModel(global_model_);\n depth_undistortion_estimation_->setMaxThreads(8);\n }\n\n inline void\n addCheckerboardViews (const CheckerboardViews::Ptr & rgbd_cb)\n {\n cb_views_vec_.push_back(rgbd_cb);\n }\n\n inline void\n setLocalModel (const LocalModel::Ptr & model)\n {\n local_model_ = model;\n local_matrix_ = boost::make_shared(local_model_);\n }\n\n const LocalModel::Ptr &\n localModel () const\n {\n\treturn local_model_;\n }\n\n inline void\n setGlobalModel (const GlobalModel::Ptr & model)\n {\n global_model_ = model;\n global_matrix_ = boost::make_shared(global_model_);\n }\n\n const GlobalModel::Ptr &\n globalModel () const\n {\n\treturn global_model_;\n }\n\n void\n perform();\n\n void\n optimize();\n\n void\n publishData() const;\n\nprotected:\n\n void\n estimateInitialTransform ();\n\n void\n estimateTransform (const std::vector & rgbd_cb_vec);\n\n void\n optimizeTransform (const std::vector & rgbd_cb_vec);\n\n void\n optimizeAll (const std::vector & rgbd_cb_vec);\n\n void\n addData_ (const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud,\n std::vector & vec);\n\n PinholeSensor::Ptr color_sensor_;\n KinectDepthSensor::Ptr depth_sensor_;\n\n std::vector cb_vec_;\n\n Publisher::Ptr publisher_;\n\n bool estimate_depth_und_model_;\n bool estimate_initial_trasform_;\n int ratio_;\n\n LocalModel::Ptr local_model_;\n GlobalModel::Ptr global_model_;\n\n LocalMatrixPCL::Ptr local_matrix_;\n GlobalMatrixPCL::Ptr global_matrix_;\n\n DepthUndistortionEstimation::Ptr depth_undistortion_estimation_;\n\n std::vector data_vec_;\n std::vector test_vec_;\n\n std::vector cb_views_vec_;\n std::vector depth_data_vec_;\n\n std::vector depth_intrinsics_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_CALIBRATION_H_ */\n" }, { "path": "include/rgbd_calibration/calibration_node.h", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef RGBD_CALIBRATION_CALIBRATION_NODE_H_\n#define RGBD_CALIBRATION_CALIBRATION_NODE_H_\n\n#include \n\n#include \n\n#include \n#include \n\n#include \n\n#include \n#include \n\nnamespace calibration\n{\n\nclass CalibrationNode\n{\npublic:\n\n CalibrationNode (ros::NodeHandle & node_handle);\n\n virtual\n ~CalibrationNode ()\n {\n // Do nothing\n }\n\n virtual bool\n initialize ();\n\n virtual void\n spin () = 0;\n\n static Checkerboard::Ptr\n createCheckerboard (const calibration_msgs::CheckerboardMsg::ConstPtr & msg,\n int id);\n\n static Checkerboard::Ptr\n createCheckerboard (const calibration_msgs::CheckerboardMsg & msg,\n int id);\n\nprotected:\n\n void\n checkerboardArrayCallback (const calibration_msgs::CheckerboardArray::ConstPtr & msg);\n\n bool\n waitForMessages () const;\n\n ros::NodeHandle node_handle_;\n\n // TODO find another way to get checkerboards\n ros::Subscriber checkerboards_sub_;\n calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;\n\n std::vector cb_vec_;\n\n std::string camera_calib_url_;\n std::string camera_name_;\n std::string depth_camera_calib_url_;\n std::string depth_camera_name_;\n\n bool has_initial_transform_;\n Transform initial_transform_;\n\n int downsample_ratio_;\n\n Size2 undistortion_matrix_cell_size_;\n Size2 images_size_;\n\n Calibration::Ptr calibration_;\n Publisher::Ptr publisher_;\n\n PinholeSensor::Ptr color_sensor_;\n KinectDepthSensor::Ptr depth_sensor_;\n\n std::vector depth_error_coeffs_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_CALIBRATION_NODE_H_ */\n" }, { "path": "include/rgbd_calibration/calibration_test.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_CALIBRATION_TEST_H_\n#define RGBD_CALIBRATION_CALIBRATION_TEST_H_\n\n#include \n#include \n\n#include \n#include \n#include \n\n//#define HERRERA\n//#define UNCALIBRATED\n//#define SKEW\n\nnamespace calibration\n{\n\nclass CalibrationTest\n{\npublic:\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n void setColorSensor(const PinholeSensor::Ptr & color_sensor)\n {\n color_sensor_ = color_sensor;\n }\n\n void setDepthSensor(const KinectDepthSensorBase::Ptr & depth_sensor)\n {\n depth_sensor_ = depth_sensor;\n }\n\n void setCheckerboards(const std::vector & cb_vec)\n {\n cb_vec_ = cb_vec;\n }\n\n void setPublisher(const Publisher::Ptr & publisher)\n {\n publisher_ = publisher;\n }\n\n void setDownSampleRatio(int ratio)\n {\n assert(ratio > 0);\n ratio_ = ratio;\n }\n\n PCLCloudRGB::Ptr addData(const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud);\n\n void setLocalModel(const LocalModel::Ptr & model)\n {\n local_matrix_ = boost::make_shared(model);\n }\n\n void setGlobalModel(const GlobalModel::Ptr & model)\n {\n global_matrix_ = boost::make_shared(model);\n }\n\n void publishData() const;\n\n void visualizeClouds() const;\n\n void testPlanarityError() const;\n\n void testCheckerboardError() const;\n\n void testCube() const;\n\nprotected:\n\n boost::shared_ptr >\n extractPlaneFromCloud (const PCLCloud3::Ptr & cloud,\n const Cloud2 & depth_corners) const;\n\n PinholeSensor::Ptr color_sensor_;\n KinectDepthSensorBase::Ptr depth_sensor_;\n\n std::vector cb_vec_;\n\n Publisher::Ptr publisher_;\n\n int ratio_;\n\n LocalMatrixPCL::Ptr local_matrix_;\n GlobalMatrixPCL::Ptr global_matrix_;\n\npublic:\n\n std::vector data_vec_;\n std::vector part_und_data_vec_;\n std::vector und_data_vec_;\n\n std::map data_map_;\n std::map part_data_map_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_CALIBRATION_TEST_H_ */\n" }, { "path": "include/rgbd_calibration/checkerboard_views.h", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_\n#define RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_\n\n#include \n#include \n#include \n#include \n\n#include \n\n#include \n\nnamespace calibration\n{\n\nclass CheckerboardViews\n{\npublic:\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n CheckerboardViews(const std::string & id)\n : id_(id)\n {\n // Do nothing\n }\n\n inline void setId(const std::string & id)\n {\n id_ = id;\n }\n\n inline const std::string & id() const\n {\n return id_;\n }\n\n inline void setData(const RGBDData::ConstPtr & data)\n {\n data_ = data;\n }\n\n inline const RGBDData::ConstPtr & data() const\n {\n return data_;\n }\n\n inline void setCheckerboard(const Checkerboard::ConstPtr & checkerboard)\n {\n checkerboard_ = checkerboard;\n }\n\n inline const Checkerboard::ConstPtr & checkerboard() const\n {\n return checkerboard_;\n }\n\n inline const Checkerboard::Ptr & colorCheckerboard() const\n {\n return color_checkerboard_;\n }\n\n inline const PlanarObject::Ptr & depthPlane() const\n {\n return depth_plane_;\n }\n\n inline PinholeView::ConstPtr colorView() const\n {\n return color_view_;\n }\n\n inline DepthViewPCL::ConstPtr depthView() const\n {\n return depth_view_;\n }\n\n inline const pcl::IndicesConstPtr & planeInliers() const\n {\n return plane_inliers_;\n }\n\n void setColorView(const PinholeView::ConstPtr & color_view);\n\n void setImageCorners(const Cloud2 & image_corners);\n\n void setPlaneInliers(const pcl::IndicesConstPtr & plane_inliers,\n Scalar inliers_std_dev);\n\n void setPlaneInliers(const PlaneInfo & plane_info);\n\n void draw(cv::Mat & image) const;\n\nprotected:\n\n std::string id_;\n RGBDData::ConstPtr data_;\n Checkerboard::ConstPtr checkerboard_;\n\n PinholeView::Ptr color_view_;\n DepthViewPCL::Ptr depth_view_;\n\n Checkerboard::Ptr color_checkerboard_;\n PlanarObject::Ptr depth_plane_;\n\n pcl::IndicesConstPtr plane_inliers_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_ */\n" }, { "path": "include/rgbd_calibration/checkerboard_views_extractor.h", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_\n#define RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_\n\n#include \n#include \n\nnamespace calibration\n{\n\nclass CheckerboardViewsExtraction\n{\npublic:\n\n typedef ColorView CheckerboardView;\n typedef DepthViewEigen PlaneView;\n\n CheckerboardViewsExtraction()\n : force_(false),\n only_images_(false),\n color_sensor_pose_(Pose::Identity()),\n cb_constraint_(boost::make_shared >()),\n plane_constraint_(boost::make_shared >())\n {\n }\n\n inline void setCheckerboardVector(const std::vector & cb_vec)\n {\n cb_vec_ = cb_vec;\n }\n\n inline void addCheckerboard(const Checkerboard::ConstPtr & checkerboard)\n {\n cb_vec_.push_back(checkerboard);\n }\n\n inline void setCheckerboardConstraint(const Constraint::ConstPtr & cb_constraint)\n {\n cb_constraint_ = cb_constraint;\n }\n\n inline void setPlanarObjectConstraint(const Constraint::ConstPtr & plane_constraint)\n {\n plane_constraint_ = plane_constraint;\n }\n\n inline void setInputData(const RGBDData::ConstPtr & data)\n {\n data_ = data;\n }\n\n inline void setInputData(const std::vector & data_vec)\n {\n data_vec_ = data_vec;\n }\n\n inline void setForceAll(bool force)\n {\n force_ = force;\n }\n\n inline void setOnlyImages(bool only_images)\n {\n only_images_ = only_images;\n }\n\n inline void setColorSensorPose(const Pose & color_sensor_pose)\n {\n color_sensor_pose_ = color_sensor_pose;\n }\n\n Size1 extract(std::vector & cb_views_vec,\n bool interactive = false) const;\n\n Size1 extractAll(std::vector & cb_views_vec,\n bool interactive = false) const;\n\nprivate:\n\n Size1 extract(const RGBDData::ConstPtr & data,\n std::vector & cb_views_vec,\n bool interactive,\n bool force) const;\n\n std::vector cb_vec_;\n\n RGBDData::ConstPtr data_;\n std::vector data_vec_;\n\n bool force_;\n bool only_images_;\n Pose color_sensor_pose_;\n\n Constraint::ConstPtr cb_constraint_;\n Constraint::ConstPtr plane_constraint_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_ */\n" }, { "path": "include/rgbd_calibration/depth_undistortion_estimation.h", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#ifndef RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_\n#define RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_\n\n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace calibration\n{\n\nclass DepthUndistortionEstimation\n{\npublic:\n\n struct DepthData\n {\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n DepthData(int id)\n : id_(id),\n plane_extracted_(false)\n {\n }\n\n int id_;\n\n PCLCloud3::ConstPtr cloud_;\n Checkerboard::ConstPtr checkerboard_; // Attention: in depth coordinates!!\n\n PCLCloud3::ConstPtr undistorted_cloud_;\n PlaneInfo estimated_plane_;\n\n bool plane_extracted_;\n\n };\n\n struct OrderByDistance\n {\n inline bool operator()(const DepthData::Ptr & lhs,\n const DepthData::Ptr & rhs)\n {\n return lhs->checkerboard_->corners().container().row(2).maxCoeff()\n < rhs->checkerboard_->corners().container().row(2).maxCoeff();\n }\n };\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n DepthUndistortionEstimation()\n : max_threads_(1)\n {\n // Do nothing\n }\n\n inline void setLocalModel(const LocalModel::Ptr & local_model)\n {\n local_model_ = local_model;\n local_fit_ = boost::make_shared(local_model_);\n local_fit_->setDepthErrorFunction(depth_error_function_);\n }\n\n inline void setGlobalModel(const GlobalModel::Ptr & global_model)\n {\n global_model_ = global_model;\n global_fit_ = boost::make_shared(global_model_);\n global_fit_->setDepthErrorFunction(depth_error_function_);\n InverseGlobalModel::Data::Ptr matrix = boost::make_shared(Size2(1, 1), InverseGlobalPolynomial::IdentityCoefficients());\n inverse_global_model_ = boost::make_shared(global_model_->imageSize());\n inverse_global_model_->setMatrix(matrix);\n inverse_global_fit_ = boost::make_shared(inverse_global_model_);\n }\n\n inline void addDepthData(const DepthData::Ptr & data)\n {\n data_vec_.push_back(data);\n }\n\n inline const DepthData::Ptr & addDepthData(const PCLCloud3::ConstPtr & cloud,\n const Checkerboard::ConstPtr & checkerboard)\n {\n DepthData::Ptr data = boost::make_shared(data_vec_.size() + 1);\n data->cloud_ = cloud;\n data->checkerboard_ = checkerboard;\n addDepthData(data);\n return data_vec_.back();\n }\n\n inline void setDepthData(const std::vector & data_vec)\n {\n data_vec_ = data_vec;\n }\n\n void estimateLocalModel();\n\n void estimateLocalModelReverse();\n\n void optimizeLocalModel(const Polynomial & depth_error_function);\n\n void estimateGlobalModel();\n\n inline void setMaxThreads(size_t max_threads)\n {\n assert(max_threads > 0);\n max_threads_ = max_threads;\n }\n\n Eigen::Matrix getLocalSamples(Size1 x_index,\n Size1 y_index) const\n {\n const LocalMatrixFitPCL::DataBin & samples = local_fit_->getSamples(x_index, y_index);\n Eigen::Matrix eigen_samples(samples.size(), 3);\n\n for (Size1 i = 0; i < samples.size(); ++i)\n {\n const LocalMatrixFitPCL::Data & sample = samples[i];\n eigen_samples.row(i) << sample.x_, sample.y_, sample.weight_;\n }\n\n return eigen_samples;\n }\n\n Eigen::Matrix getGlobalSamples(Size1 x_index,\n Size1 y_index) const\n {\n const GlobalMatrixFitPCL::DataBin & samples = global_fit_->getSamples(x_index, y_index);\n Eigen::Matrix eigen_samples(samples.size(), 3);\n\n for (Size1 i = 0; i < samples.size(); ++i)\n {\n const GlobalMatrixFitPCL::Data & sample = samples[i];\n eigen_samples.row(i) << sample.x_, sample.y_, sample.weight_;\n }\n\n return eigen_samples;\n }\n\n void setDepthErrorFunction(const Polynomial & depth_error_function)\n {\n depth_error_function_ = depth_error_function;\n }\n\nprivate:\n\n bool extractPlane(const Checkerboard & color_cb,\n const PCLCloud3::ConstPtr & cloud,\n const Point3 & color_cb_center,\n PlaneInfo & plane_info);\n\n Size1 max_threads_;\n\n LocalModel::Ptr local_model_;\n LocalMatrixFitPCL::Ptr local_fit_;\n\n GlobalModel::Ptr global_model_;\n GlobalMatrixFitPCL::Ptr global_fit_;\n\n InverseGlobalModel::Ptr inverse_global_model_;\n InverseGlobalMatrixFitEigen::Ptr inverse_global_fit_;\n\n std::vector data_vec_;\n\n std::map plane_info_map_;\n Polynomial depth_error_function_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_ */\n" }, { "path": "include/rgbd_calibration/globals.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_GLOBALS_H_\n#define RGBD_CALIBRATION_GLOBALS_H_\n\n#include \n#include \n#include \n\n//#include \n#include \n#include \n\n#include \n#include \n\n#define G_POLY_DEGREE 2\n#define G_POLY_MIN_DEGREE 1\n#define L_POLY_DEGREE 2\n#define L_POLY_MIN_DEGREE 0\n\n//#define KINECT_FOV_X 58.6 //Scalar(DEG2RAD(57.0)) 58.6\n//#define KINECT_FOV_Y 45.7 //Scalar(DEG2RAD(43.5)) 45.7\n\n//#define KINECT_ERROR_POLY Vector3(0.0, 0.0, 0.0035)\n\nnamespace calibration\n{\n\ntypedef Polynomial LocalPolynomial;\ntypedef Polynomial GlobalPolynomial;\ntypedef Polynomial InverseGlobalPolynomial;\n\ntypedef PolynomialMatrixSmoothModel LocalModel;\ntypedef PolynomialMatrixPCL LocalMatrixPCL;\ntypedef PolynomialMatrixEigen LocalMatrixEigen;\ntypedef PolynomialMatrixSmoothModelFitPCL LocalMatrixFitPCL;\ntypedef PolynomialMatrixSmoothModelFitEigen LocalMatrixFitEigen;\n\ntypedef PolynomialMatrixSmoothModel GlobalModel;\ntypedef PolynomialMatrixPCL GlobalMatrixPCL;\ntypedef PolynomialMatrixEigen GlobalMatrixEigen;\ntypedef PolynomialMatrixSmoothModelFitPCL GlobalMatrixFitPCL;\ntypedef PolynomialMatrixSmoothModelFitEigen GlobalMatrixFitEigen;\n\ntypedef PolynomialMatrixSimpleModel InverseGlobalModel;\ntypedef PolynomialMatrixPCL InverseGlobalMatrixPCL;\ntypedef PolynomialMatrixEigen InverseGlobalMatrixEigen;\ntypedef PolynomialMatrixSimpleModelFitPCL InverseGlobalMatrixFitPCL;\ntypedef PolynomialMatrixSimpleModelFitEigen InverseGlobalMatrixFitEigen;\n\n//typedef PolynomialFunctionModel::Data UFunctionData;\n//typedef PolynomialFunctionPCL UFunctionPCL;\n//typedef PolynomialFunctionEigen UFunctionEigen;\n//typedef PolynomialFunctionFitPCL UFunctionFitPCL;\n//typedef PolynomialFunctionFitEigen UFunctionFitEigen;\n\ntypedef TwoStepsModel UndistortionModel;\ntypedef TwoStepsUndistortionEigen UndistortionEigen;\ntypedef TwoStepsUndistortionPCL UndistortionPCL;\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_GLOBALS_H_ */\n" }, { "path": "include/rgbd_calibration/offline_calibration_node.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_\n#define RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_\n\n#include \n\nnamespace calibration\n{\n\nenum DepthType\n{\n KINECT1_DEPTH,\n SWISS_RANGER_DEPTH\n};\n\nclass OfflineCalibrationNode : public CalibrationNode\n{\npublic:\n\n OfflineCalibrationNode (ros::NodeHandle & node_handle);\n\n virtual\n ~OfflineCalibrationNode()\n {\n // Do nothing\n }\n\n virtual void\n spin ();\n\nprotected:\n\n int instances_;\n int starting_index_;\n\n std::string path_;\n std::string image_extension_;\n std::string image_filename_;\n std::string cloud_filename_;\n\n DepthType depth_type_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_ */\n" }, { "path": "include/rgbd_calibration/plane_based_extrinsic_calibration.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_\n#define RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_\n\n#include \n#include \n#include \n\nnamespace calibration\n{\n\nclass PlaneBasedExtrinsicCalibration\n{\npublic:\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n void addData(size_t index,\n const Sensor::Ptr & sensor,\n const PlanarObject::ConstPtr & data)\n {\n assert(index < data_map_.size());\n data_map_[index][sensor] = data;\n }\n\n void setSize(size_t size)\n {\n data_map_.resize(size);\n }\n\n size_t size() const\n {\n return data_map_.size();\n }\n\n size_t appendData(const std::map & data)\n {\n data_map_.push_back(data);\n return data_map_.size();\n }\n\n void setMainSensor(const Sensor::Ptr & world)\n {\n world_ = world;\n }\n\n void perform()\n {\n std::map calib_map;\n for (size_t i = 0; i < data_map_.size(); ++i)\n {\n std::map & data_map = data_map_[i];\n if (data_map.find(world_) == data_map.end())\n continue;\n const PlanarObject::ConstPtr & world_data = data_map_[i][world_];\n for (std::map::const_iterator it = data_map.begin(); it != data_map.end(); ++it)\n {\n const Sensor::Ptr & sensor = it->first;\n const PlanarObject::ConstPtr & sensor_data = it->second;\n if (sensor != world_)\n calib_map[sensor].addPair(world_data->plane(), sensor_data->plane());\n }\n\n }\n\n for (std::map::iterator it = calib_map.begin(); it != calib_map.end(); ++it)\n {\n const Sensor::Ptr & sensor = it->first;\n\n if (calib_map[sensor].getPairNumber() > 5)\n {\n sensor->setParent(world_);\n sensor->setPose(calib_map[sensor].estimateTransform());\n }\n else\n sensor->setParent(Sensor::ConstPtr());\n }\n\n }\n\n void optimize()\n {\n // TODO implement!!\n }\n\nprotected:\n\n Sensor::Ptr world_;\n std::vector > data_map_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_ */\n" }, { "path": "include/rgbd_calibration/publisher.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_PUBLISHER_H_\n#define RGBD_CALIBRATION_PUBLISHER_H_\n\n#include \n#include \n#include \n\nnamespace calibration\n{\n\nclass Publisher\n{\nprivate:\n\n struct CheckerboardPublisherSet\n {\n ros::Publisher marker_pub_;\n };\n\n struct DataPublisherSet\n {\n ros::Publisher cloud_pub_;\n ros::Publisher rgbd_pub_;\n };\n\npublic:\n\n typedef boost::shared_ptr Ptr;\n typedef boost::shared_ptr ConstPtr;\n\n Publisher(const ros::NodeHandle & node_handle);\n\n void publish(const CheckerboardViews & rgbd,\n const std::string & prefix = \"\");\n\n void publish(const RGBDData & rgbd);\n\n void publish(const Cloud3 & point_set,\n const BaseObject & frame);\n\n void publishTF(const BaseObject & object);\n\nprivate:\n\n ros::NodeHandle node_handle_;\n ros::Publisher marker_pub_;\n tf::TransformBroadcaster tf_pub_;\n\n std::map d_pub_map_;\n std::map c_pub_map_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_PUBLISHER_H_ */\n" }, { "path": "include/rgbd_calibration/simulation_node.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_SIMULATION_NODE_H_\n#define RGBD_CALIBRATION_SIMULATION_NODE_H_\n\n#include \n\nnamespace calibration\n{\n\nclass SimulationNode : public CalibrationNode\n{\npublic:\n\n SimulationNode(ros::NodeHandle & node_handle);\n\n virtual void spin();\n\nprotected:\n\n double depth_error_;\n double image_error_;\n\n double min_distance_;\n double max_distance_;\n double step_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_SIMULATION_NODE_H_ */\n" }, { "path": "include/rgbd_calibration/test_node.h", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#ifndef RGBD_CALIBRATION_TEST_NODE_H_\n#define RGBD_CALIBRATION_TEST_NODE_H_\n\n#include \n\n#include \n\n#include \n#include \n\n#include \n\n#include \n#include \n\nnamespace calibration\n{\n\nclass TestNode\n{\npublic:\n\n enum DepthType\n {\n KINECT1_DEPTH,\n SWISS_RANGER_DEPTH\n };\n\n TestNode(ros::NodeHandle & node_handle);\n\n virtual ~TestNode()\n {\n // Do nothing\n }\n\n virtual bool initialize();\n\n virtual void spin();\n virtual void spin2();\n virtual void spin3();\n\n static Checkerboard::Ptr createCheckerboard(const calibration_msgs::CheckerboardMsg::ConstPtr & msg,\n int id);\n\n static Checkerboard::Ptr createCheckerboard(const calibration_msgs::CheckerboardMsg & msg,\n int id);\n\nprotected:\n\n void checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg);\n\n bool waitForMessages() const;\n\n ros::NodeHandle node_handle_;\n\n // TODO find another way to get checkerboards\n ros::Subscriber checkerboards_sub_;\n calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;\n\n std::vector cb_vec_;\n\n std::string camera_calib_url_;\n std::string camera_name_;\n std::string depth_camera_calib_url_;\n std::string depth_camera_name_;\n\n Pose camera_pose_;\n\n std::string local_matrix_file_;\n std::string global_matrix_file_;\n\n int downsample_ratio_;\n\n PinholeSensor::Ptr color_sensor_;\n KinectDepthSensor::Ptr depth_sensor_;\n\n Publisher::Ptr publisher_;\n\n std::vector depth_intrinsics_;\n int instances_;\n int starting_index_;\n\n std::string path_;\n std::string image_extension_;\n std::string image_filename_;\n std::string cloud_filename_;\n\n bool only_show_;\n\n CalibrationTest::Ptr test_;\n\n Size2 images_size_;\n DepthType depth_type_;\n\n};\n\n} /* namespace calibration */\n#endif /* RGBD_CALIBRATION_TEST_NODE_H_ */\n" }, { "path": "launch/kinect2_507_tro.launch", "content": "\n\n\n\n\n\n\n \n \n \n \n \n \n\n \n \n \n\n \n \n \n \n \n\t\n\t \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n camera_pose:\n translation: {x: 0.0, y: 0.0, z: 0.0}\n rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}\n \n depth_error_function: [0.002, -0.001, 0.002]\n \n depth_image:\n cols: 512\n rows: 424\n \n undistortion_matrix:\n cell_size_x: 2\n cell_size_y: 2\n \n \n \n \n \n\n \n \n\n\n\n\n" }, { "path": "launch/kinect2_507_tro_test.launch", "content": "\n\n\n\n \n\n\n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\t\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n depth_image:\n cols: 512\n rows: 424\n \n \n\n \n \n\n\n\n\n" }, { "path": "launch/kinect_47A_tro.launch", "content": "\n\n\n\n\n\n\n \n \n \n \n \n \n\n \n \n \n\n \n \n \n \n \n\t\n\t \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n camera_pose:\n translation: {x: 0.0, y: 0.0, z: 0.0}\n rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}\n \n depth_error_function: [-0.00029, 0.00037, 0.001365]\n \n undistortion_matrix:\n cell_size_x: 2\n cell_size_y: 2\n \n \n \n \n \n \n\n \n \n\n\n\n\n" }, { "path": "launch/kinect_47A_tro_test.launch", "content": "\n\n\n\n \n\n\n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n\t\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n\n\n\n\n" }, { "path": "launch/kinect_data_collection.launch", "content": "\n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "launch/pepper_tro.launch", "content": "\r\n\r\n\r\n\r\n\r\n\r\n \r\n \r\n \r\n \r\n \r\n \r\n\r\n \r\n \r\n \r\n\r\n \r\n \r\n \r\n\r\n \r\n\r\n\t \r\n \r\n\r\n \r\n \r\n \r\n \r\n \r\n \r\n\r\n \r\n \r\n \r\n\r\n \r\n \r\n \r\n\r\n \r\n camera_pose:\r\n translation: {x: -0.039, y: 0.044, z: -0.035}\r\n rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}\r\n\r\n depth_image:\r\n cols: 320\r\n rows: 240\r\n\r\n depth_error_function: [-0.0029, 0.0037, 0.01365]\r\n\r\n undistortion_matrix:\r\n cell_size_x: 2\r\n cell_size_y: 2\r\n \r\n \r\n\r\n \r\n\r\n \r\n\r\n \r\n \r\n\r\n\r\n\r\n\r\n" }, { "path": "msg/Acquisition.msg", "content": "float32 distance\nstring info\n" }, { "path": "package.xml", "content": "\n\n rgbd_calibration\n 0.0.0\n The calibration package\n\n \n \n \n filippo\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n catkin\n\n cmake_modules\n\n roscpp\n calibration_common\n geometry_msgs\n eigen_conversions\n camera_info_manager\n cv_bridge\n image_transport\n pcl_ros\n kinect\n\n message_generation\n message_runtime\n \n roscpp\n calibration_common\n geometry_msgs\n eigen_conversions\n camera_info_manager\n cv_bridge\n image_transport\n pcl_ros\n kinect\n\n \n \n \n \n\n \n\n \n\n" }, { "path": "src/rgbd_calibration/calibration.cpp", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n\n#define RGBD_INFO(id, msg) ROS_INFO_STREAM(\"RGBD \" << id << \": \" << msg)\n\nnamespace calibration\n{\n\nclass CheckerboardDistanceConstraint : public Constraint\n{\n\npublic:\n\n CheckerboardDistanceConstraint (Scalar distance,\n const Point3 & from = Point3::Zero())\n : distance_(distance),\n from_(from)\n {\n // Do nothing\n }\n\n virtual\n ~CheckerboardDistanceConstraint()\n {\n // Do nothing\n }\n\n inline virtual bool\n isValid (const Checkerboard & checkerboard) const\n {\n return (checkerboard.center() - from_).norm() <= distance_;\n }\n\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\nprivate:\n\n Scalar distance_;\n Point3 from_;\n\n};\n\nvoid\nCalibration::publishData () const\n{\n if (not publisher_)\n return;\n\n publisher_->publishTF(*depth_sensor_);\n publisher_->publishTF(*color_sensor_);\n\n for (size_t i = 0; i < test_vec_.size(); ++i)\n publisher_->publish(*test_vec_[i]);\n\n for (size_t i = 0; i < cb_views_vec_.size(); ++i)\n publisher_->publish(*cb_views_vec_[i]);\n\n}\n\nvoid\nCalibration::addData_ (const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud,\n std::vector & vec)\n{\n RGBDData::Ptr data(boost::make_shared(data_vec_.size() + 1));\n data->setColorSensor(color_sensor_);\n data->setDepthSensor(depth_sensor_);\n\n //cv::Mat rectified;\n //color_sensor_->cameraModel()->rectifyImage(image, rectified);\n data->setColorData(image);\n\n if (ratio_ > 1)\n {\n PCLCloud3::Ptr new_cloud(boost::make_shared());\n new_cloud->resize(cloud->size() / (ratio_ * ratio_));\n new_cloud->header = cloud->header;\n new_cloud->width = cloud->width / ratio_;\n new_cloud->height = cloud->height / ratio_;\n new_cloud->is_dense = cloud->is_dense;\n\n PCLPoint3 zero(0, 0, 0);\n float nan = std::numeric_limits::quiet_NaN();\n PCLPoint3 bad_point(nan, nan, nan);\n\n for (Size1 i = 0; i < new_cloud->height; ++i)\n {\n for (Size1 j = 0; j < new_cloud->width; ++j)\n {\n new_cloud->at(j, i) = zero;\n int count = 0;\n for (Size1 di = 0; di < ratio_; ++di)\n {\n for (Size1 dj = 0; dj < ratio_; ++dj)\n {\n const PCLPoint3 & p = cloud->at(j * ratio_ + dj, i * ratio_ + di);\n if (pcl::isFinite(p))\n {\n ++count;\n new_cloud->at(j, i).x += p.x;\n new_cloud->at(j, i).y += p.y;\n new_cloud->at(j, i).z += p.z;\n }\n }\n }\n if (count > 0)\n {\n new_cloud->at(j, i).x /= count;\n new_cloud->at(j, i).y /= count;\n new_cloud->at(j, i).z /= count;\n }\n else\n {\n new_cloud->at(j, i) = bad_point;\n new_cloud->is_dense = false;\n }\n }\n }\n data->setDepthData(*new_cloud);\n }\n else\n {\n data->setDepthData(*cloud);\n }\n\n vec.push_back(data);\n}\n\nvoid\nCalibration::addData (const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud)\n{\n addData_(image, cloud, data_vec_);\n}\n\n//void\n//Calibration::addTestData (const cv::Mat & image,\n// const PCLCloud3::ConstPtr & cloud)\n//{\n// addData_(image, cloud, test_vec_);\n//}\n\nvoid\nCalibration::perform ()\n{\n if (estimate_initial_trasform_ or not color_sensor_->parent())\n estimateInitialTransform();\n\n if (estimate_depth_und_model_)\n {\n CheckerboardViewsExtraction cb_extractor;\n cb_extractor.setColorSensorPose(color_sensor_->pose());\n cb_extractor.setCheckerboardVector(cb_vec_);\n cb_extractor.setInputData(data_vec_);\n cb_extractor.setOnlyImages(true);\n cb_extractor.extractAll(cb_views_vec_);\n\n ROS_INFO_STREAM(cb_views_vec_.size());\n\n for (Size1 i = 0; i < cb_views_vec_.size(); ++i)\n {\n CheckerboardViews & cb_views = *cb_views_vec_[i];\n Checkerboard::Ptr cb = boost::make_shared(*cb_views.colorCheckerboard());\n cb->transform(color_sensor_->pose());\n depth_data_vec_.push_back(depth_undistortion_estimation_->addDepthData(cb_views.data()->depthData(), cb));\n }\n\n ROS_INFO_STREAM(\"Estimating undistortion map...\");\n depth_undistortion_estimation_->estimateLocalModel();\n ROS_INFO_STREAM(\"Recomputing undistortion map...\");\n depth_undistortion_estimation_->estimateLocalModelReverse();\n ROS_INFO_STREAM(\"Estimating global error correction map...\");\n depth_undistortion_estimation_->estimateGlobalModel();\n\n for (Size1 i = 0; i < cb_views_vec_.size(); ++i)\n {\n CheckerboardViews & cb_views = *cb_views_vec_[i];\n if (depth_data_vec_[i]->plane_extracted_)\n cb_views.setPlaneInliers(depth_data_vec_[i]->estimated_plane_);\n else\n cb_views_vec_[i].reset();\n }\n\n// PolynomialUndistortionMatrixIO io;\n//// io.write(*local_matrix_->model(), \"/tmp/local_matrix.txt\");\n// int index = 0;\n// for (Scalar i = 1.0; i < 5.5; i += 0.125, ++index)\n// {\n// Scalar max;\n// std::stringstream ss;\n// ss << \"/tmp/matrix_\"<< index << \".png\";\n// io.writeImageAuto(*local_matrix_->model(), i, ss.str(), max);\n// ROS_INFO_STREAM(\"Max \" << i << \": \" << max);\n// }\n\n// std::vector > pixels;\n// pixels.push_back(std::make_pair(0, 0));\n// pixels.push_back(std::make_pair(40/ratio_/local_model_->binSize().x(), 480/ratio_/local_model_->binSize().y()));\n// pixels.push_back(std::make_pair(320/ratio_/local_model_->binSize().x(), 240/ratio_/local_model_->binSize().y()));\n// pixels.push_back(std::make_pair(560/ratio_/local_model_->binSize().x(), 400/ratio_/local_model_->binSize().y()));\n//\n// for (int i = 0; i < pixels.size(); ++i)\n// {\n// std::cout << \"Local Polynomial at (\" << pixels[i].first << \", \" << pixels[i].second << \"): \" << local_model_->polynomial(pixels[i].first, pixels[i].second).transpose() << std::endl;\n//\n// std::stringstream ss;\n// ss << \"/tmp/local_samples_\" << pixels[i].first << \"_\" << pixels[i].second << \".txt\";\n//\n// std::fstream fs;\n// fs.open(ss.str().c_str(), std::fstream::out);\n// fs << depth_undistortion_estimation_->getLocalSamples(pixels[i].first, pixels[i].second) << std::endl;\n// fs.close();\n// }\n//\n// for (int i = 0; i < 4; ++i)\n// {\n// std::cout << \"Global Polynomial at (\" << i / 2 << \", \" << i % 2 << \"): \" << global_model_->polynomial(i / 2, i % 2).transpose() << std::endl;\n//\n// std::stringstream ss;\n// ss << \"/tmp/global_samples_\" << i / 2 << \"_\" << i % 2 << \".txt\";\n//\n// std::fstream fs;\n// fs.open(ss.str().c_str(), std::fstream::out);\n// fs << depth_undistortion_estimation_->getGlobalSamples(i / 2, i % 2) << std::endl;\n// fs.close();\n// }\n\n }\n\n estimateTransform(cb_views_vec_);\n\n}\n\nvoid\nCalibration::estimateInitialTransform ()\n{\n CheckerboardViewsExtraction cb_extractor;\n cb_extractor.setCheckerboardVector(cb_vec_);\n cb_extractor.setCheckerboardConstraint(boost::make_shared(2.0));\n\n std::vector cb_views_vec;\n\n for (Size1 i = 0; i < data_vec_.size() and cb_views_vec.size() < 10; ++i)\n {\n Size1 index = rand() % data_vec_.size();\n cb_extractor.setInputData(data_vec_[index]);\n cb_extractor.extract(cb_views_vec, true);\n }\n\n estimateTransform(cb_views_vec);\n}\n\nvoid\nCalibration::estimateTransform (const std::vector & cb_views_vec)\n{\n PlaneBasedExtrinsicCalibration calib;\n calib.setMainSensor(depth_sensor_);\n calib.setSize(cb_views_vec.size());\n\n int index = 0;\n\n for (Size1 i = 0; i < cb_views_vec.size(); ++i)\n {\n if (cb_views_vec[i])\n {\n calib.addData(index, color_sensor_, cb_views_vec[i]->colorCheckerboard());\n calib.addData(index++, depth_sensor_, cb_views_vec[i]->depthPlane());\n }\n }\n\n calib.setSize(index);\n calib.perform();\n}\n\nclass TransformError\n{\npublic:\n\n TransformError(const PinholeCameraModel::ConstPtr & camera_model,\n const Checkerboard::ConstPtr & checkerboard,\n const Cloud2 & image_corners,\n const Plane & depth_plane,\n const Polynomial & depth_error_function)\n : camera_model_(camera_model),\n checkerboard_(checkerboard),\n image_corners_(image_corners),\n depth_plane_(depth_plane),\n depth_error_function_(depth_error_function)\n {\n }\n\n template \n bool operator ()(const T * const color_sensor_pose,\n const T * const checkerboard_pose,\n T * residuals) const\n {\n typename Types::Vector3 color_sensor_r_vec(color_sensor_pose[0], color_sensor_pose[1], color_sensor_pose[2]);\n typename Types::AngleAxis color_sensor_r(color_sensor_r_vec.norm(), color_sensor_r_vec.normalized());\n typename Types::Translation3 color_sensor_t(color_sensor_pose[3], color_sensor_pose[4], color_sensor_pose[5]);\n\n typename Types::Transform color_sensor_pose_eigen = color_sensor_t * color_sensor_r;\n\n typename Types::Vector3 checkerboard_r_vec(checkerboard_pose[0], checkerboard_pose[1], checkerboard_pose[2]);\n typename Types::AngleAxis checkerboard_r(checkerboard_r_vec.norm(), checkerboard_r_vec.normalized());\n typename Types::Translation3 checkerboard_t(checkerboard_pose[3], checkerboard_pose[4], checkerboard_pose[5]);\n\n typename Types::Transform checkerboard_pose_eigen = checkerboard_t * checkerboard_r;\n\n typename Types::Cloud3 cb_corners(checkerboard_->corners().size());\n cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast();\n\n typename Types::Plane depth_plane(depth_plane_.normal().cast(), T(depth_plane_.offset()));\n typename Types::Cloud2 reprojected_corners = camera_model_->project3dToPixel2(cb_corners);\n\n cb_corners.transform(color_sensor_pose_eigen);\n\n Polynomial depth_error_function(depth_error_function_.coefficients().cast());\n\n for (Size1 i = 0; i < cb_corners.elements(); ++i)\n {\n residuals[2 * i] = T((reprojected_corners[i] - image_corners_[i].cast()).norm() / 0.5);\n residuals[2 * i + 1] = T(depth_plane.absDistance(cb_corners[i])\n / ceres::poly_eval(depth_error_function.coefficients(), cb_corners[i].z())); // TODO use line-of-sight error\n }\n\n return true;\n }\n\nprivate:\n\n const PinholeCameraModel::ConstPtr & camera_model_;\n const Checkerboard::ConstPtr & checkerboard_;\n const Cloud2 & image_corners_;\n const Plane & depth_plane_;\n const Polynomial depth_error_function_;\n\n};\n\nvoid Calibration::optimizeTransform(const std::vector & cb_views_vec)\n{\n ceres::Problem problem;\n Eigen::Matrix data(cb_views_vec.size(), 6);\n\n AngleAxis rotation(color_sensor_->pose().linear());\n Translation3 translation(color_sensor_->pose().translation());\n\n Eigen::Matrix transform;\n transform.head<3>() = rotation.angle() * rotation.axis();\n transform.tail<3>() = translation.vector();\n\n for (size_t i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n\n rotation = AngleAxis(cb_views.colorCheckerboard()->pose().linear());\n data.row(i).head<3>() = rotation.angle() * rotation.axis();\n data.row(i).tail<3>() = cb_views.colorCheckerboard()->pose().translation();\n\n TransformError * error = new TransformError(color_sensor_->cameraModel(),\n cb_views.checkerboard(),\n cb_views.colorView()->points(),\n cb_views.depthPlane()->plane(),\n depth_sensor_->depthErrorFunction());\n\n typedef ceres::AutoDiffCostFunction TransformCostFunction;\n\n ceres::CostFunction * cost_function = new TransformCostFunction(error, 2 * cb_views.checkerboard()->size());\n problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(1.0), transform.data(), data.row(i).data());\n }\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_SCHUR;\n options.max_num_iterations = 100;\n options.minimizer_progress_to_stdout = true;\n options.num_threads = 8;\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n\n rotation.angle() = transform.head<3>().norm();\n rotation.axis() = transform.head<3>().normalized();\n translation.vector() = transform.tail<3>();\n\n color_sensor_->setPose(translation * rotation);\n\n}\n\nclass ReprojectionError\n{\npublic:\n\n ReprojectionError (const PinholeCameraModel::ConstPtr & camera_model,\n const Checkerboard::ConstPtr & checkerboard,\n const Cloud2 & image_corners)\n : camera_model_(camera_model),\n checkerboard_(checkerboard),\n image_corners_(image_corners)\n {\n // Do nothing\n }\n\n template \n typename Types::Pose toEigen(const T * const q, const T * const t) const\n {\n typename Types::Quaternion rotation(q[0], q[1], q[2], q[3]);\n typename Types::Translation3 translation(t[0], t[1], t[2]);\n return translation * rotation;\n }\n\n template \n bool\n\toperator () (const T * const checkerboard_pose_q,\n const T * const checkerboard_pose_t,\n T * residuals) const\n {\n typename Types::Pose checkerboard_pose_eigen = toEigen(checkerboard_pose_q, checkerboard_pose_t);\n\n typename Types::Cloud3 cb_corners(checkerboard_->corners().size());\n cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast();\n\n typename Types::Cloud2 reprojected_corners(checkerboard_->corners().size());\n for (Size1 i = 0; i < cb_corners.elements(); ++i)\n reprojected_corners[i] = camera_model_->project3dToPixel2(cb_corners[i]);\n\n Eigen::Map > residual_map(residuals, 2, cb_corners.elements());\n residual_map = (reprojected_corners.container() - image_corners_.container().cast()) / (0.5 * std::sqrt(T(cb_corners.elements())));\n\n return true;\n }\n\nprivate:\n\n const PinholeCameraModel::ConstPtr & camera_model_;\n const Checkerboard::ConstPtr & checkerboard_;\n Cloud2 image_corners_;\n};\n\nclass TransformDistortionError\n{\npublic:\n\n TransformDistortionError(const PinholeCameraModel::ConstPtr & camera_model,\n\t\t \t \t \t \t const KinectDepthCameraModel::ConstPtr & depth_camera_model,\n const Checkerboard::ConstPtr & checkerboard,\n const Cloud3 & depth_points,\n const Indices & plane_indices,\n const Polynomial & depth_error_function,\n const Size2 & images_size)\n : camera_model_(camera_model),\n depth_camera_model_(depth_camera_model),\n checkerboard_(checkerboard),\n depth_points_(depth_points),\n plane_indices_(plane_indices),\n depth_error_function_(depth_error_function),\n images_size_(images_size)\n {\n // Do nothing\n }\n\n template \n typename Types::Pose toEigen(const T * const q, const T * const t) const\n {\n typename Types::Quaternion rotation(q[0], q[1], q[2], q[3]);\n typename Types::Translation3 translation(t[0], t[1], t[2]);\n return translation * rotation;\n }\n\n template \n bool operator ()(const T * const color_sensor_pose_q,\n const T * const color_sensor_pose_t,\n const T * const global_undistortion,\n const T * const checkerboard_pose_q,\n const T * const checkerboard_pose_t,\n const T * const delta,\n T * residuals) const\n {\n typename Types::Pose color_sensor_pose_eigen = toEigen(color_sensor_pose_q, color_sensor_pose_t);\n typename Types::Pose checkerboard_pose_eigen = toEigen(checkerboard_pose_q, checkerboard_pose_t);\n\n const int DEGREE = MathTraits::Degree;\n const int MIN_DEGREE = MathTraits::MinDegree;\n const int SIZE = MathTraits::Size;//DEGREE - MIN_DEGREE + 1;\n typedef MathTraits::Coefficients Coefficients;\n\n Size1 index = 0;\n\n Coefficients c1, c2, c3;\n for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)\n c1[i] = global_undistortion[index++];\n for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)\n c2[i] = global_undistortion[index++];\n for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)\n c3[i] = global_undistortion[index++];\n\n Polynomial p1(c1);\n Polynomial p2(c2);\n Polynomial p3(c3);\n\n Eigen::Matrix A(SIZE, SIZE);\n Eigen::Matrix b(SIZE, 1);\n for (int i = 0; i < SIZE; ++i)\n {\n T x(i + 1);\n T y = p2.evaluate(x) + p3.evaluate(x) - p1.evaluate(x);\n T tmp(1.0);\n for (int j = 0; j < MIN_DEGREE; ++j)\n tmp *= x;\n for (int j = 0; j < SIZE; ++j)\n {\n A(i, j) = tmp;\n tmp *= x;\n }\n b[i] = y;\n }\n\n Eigen::Matrix x = A.colPivHouseholderQr().solve(b);\n\n GlobalModel::Data::Ptr global_data = boost::make_shared(Size2(2, 2));\n for (int i = 0; i < 3 * MathTraits::Size; ++i)\n global_data->container().data()[0 * MathTraits::Size + i] = global_undistortion[i];\n for (int i = 0; i < SIZE; ++i)\n global_data->container().data()[3 * MathTraits::Size + i] = x[i];\n\n GlobalModel::Ptr global_model = boost::make_shared(images_size_);\n global_model->setMatrix(global_data);\n\n GlobalMatrixEigen global(global_model);\n\n typename Types::Cloud3 depth_points(depth_points_.size());\n depth_points.container() = depth_points_.container().cast();\n\n const cv::Matx33d & K = depth_camera_model_->intrinsicMatrix();\n\n for (int j = 0; j < depth_points.size().y(); ++j)\n {\n for (int i = 0; i < depth_points.size().x(); ++i)\n {\n T z = depth_points(i, j).z();\n typename Types::Point2 normalized_pixel((i - (K(0, 2) + delta[2])) / (K(0, 0) * delta[0]),\n (j - (K(1, 2) + delta[3])) / (K(1, 1) * delta[1]));\n\n depth_points(i, j) = z * depth_camera_model_->undistort2d_(normalized_pixel).homogeneous();\n\n /*T z = depth_points(i, j).z();\n depth_points(i, j).x() = (i - (depth_camera_model_->cx() + depth_camera_model_->Tx() + delta[2])) * depth_points(i, j).z() / (depth_camera_model_->fx() * delta[0]);\n depth_points(i, j).y() = (j - (depth_camera_model_->cy() + depth_camera_model_->Ty() + delta[3])) * depth_points(i, j).z() / (depth_camera_model_->fy() * delta[1]);*/\n }\n }\n\n global.undistort(depth_points);\n\n typename Types::Cloud3 cb_corners(checkerboard_->corners().size());\n cb_corners.container() = color_sensor_pose_eigen * checkerboard_pose_eigen * checkerboard_->corners().container().cast();\n\n Polynomial depth_error_function(depth_error_function_.coefficients().cast());\n typename Types::Plane cb_plane = Types::Plane::Through(cb_corners(0, 0), cb_corners(0, 1), cb_corners(1, 0));\n\n Eigen::Map > residual_map_dist(residuals, 3, plane_indices_.size());\n for (Size1 i = 0; i < plane_indices_.size(); ++i)\n {\n Line line(Point3::Zero(), depth_points[plane_indices_[i]].normalized());\n residual_map_dist.col(i) = (line.intersectionPoint(cb_plane) - depth_points[plane_indices_[i]]) /\n (std::sqrt(T(plane_indices_.size())) * ceres::poly_eval(depth_error_function.coefficients(), depth_points[plane_indices_[i]].z()));\n }\n\n// Scalar alpha = std::acos(depth_plane.normal().dot(cb_plane.normal()));\n// if (alpha > M_PI_2)\n// alpha = M_PI - alpha;\n// residual_map_dist *= std::exp(alpha);\n\n return true;\n }\n\nprivate:\n\n const PinholeCameraModel::ConstPtr & camera_model_;\n const KinectDepthCameraModel::ConstPtr & depth_camera_model_;\n const Checkerboard::ConstPtr & checkerboard_;\n\n const Cloud3 depth_points_;\n const Indices plane_indices_;\n\n const Polynomial depth_error_function_;\n const Size2 images_size_;\n\n};\n\ntypedef ceres::NumericDiffCostFunction ReprojectionCostFunction;\n\ntypedef ceres::NumericDiffCostFunction::Size, 4, 3, 4> TransformDistortionCostFunction;\n\nvoid\nCalibration::optimizeAll (const std::vector & cb_views_vec)\n{\n ceres::Problem problem;\n Eigen::Matrix data(cb_views_vec.size(), 7);\n\n //AngleAxis rotation(color_sensor_->pose().linear());\n Quaternion rotation(color_sensor_->pose().linear());\n Translation3 translation(color_sensor_->pose().translation());\n\n Eigen::Matrix transform;\n //transform.head<3>() = rotation.angle() * rotation.axis();\n transform[0] = rotation.w();\n transform[1] = rotation.x();\n transform[2] = rotation.y();\n transform[3] = rotation.z();\n transform.tail<3>() = translation.vector();\n\n double delta[4] = {1.0, 1.0, 0.0, 0.0};\n\n for (size_t i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n\n /*rotation = AngleAxis(cb_views.colorCheckerboard()->pose().linear());\n data.row(i).head<3>() = rotation.angle() * rotation.axis();*/\n\n Quaternion rotation = Quaternion(cb_views.colorCheckerboard()->pose().linear());\n data.row(i)[0] = rotation.w();\n data.row(i)[1] = rotation.x();\n data.row(i)[2] = rotation.y();\n data.row(i)[3] = rotation.z();\n data.row(i).tail<3>() = cb_views.colorCheckerboard()->pose().translation();\n\n TransformDistortionError * error;\n ceres::CostFunction * cost_function;\n\n error = new TransformDistortionError(color_sensor_->cameraModel(),\n\t\t\t\t\t\t\t\t\t depth_sensor_->cameraModel(),\n\t\t\t\t\t\t\t\t\t cb_views.checkerboard(),\n\t\t\t\t\t\t\t\t\t PCLConversion::toPointMatrix(*cb_views.depthView()->data()),\n\t\t\t\t\t\t\t\t\t cb_views.depthView()->points(),\n\t\t\t\t\t\t\t\t\t depth_sensor_->depthErrorFunction(),\n\t\t\t\t\t\t\t\t\t global_model_->imageSize());\n\n cost_function = new TransformDistortionCostFunction(error,\n \t\t ceres::DO_NOT_TAKE_OWNERSHIP,\n\t\t\t\t\t\t\t\t\t\t\t\t\t\t3 * cb_views.depthView()->points().size());\n\n problem.AddResidualBlock(cost_function,\n NULL,//new ceres::CauchyLoss(1.0),\n transform.data(),\n transform.data() + 4,\n global_matrix_->model()->dataPtr(),\n data.row(i).data(),\n data.row(i).data() + 4,\n delta);\n\n ReprojectionError * repr_error = new ReprojectionError(color_sensor_->cameraModel(),\n cb_views.checkerboard(),\n cb_views.colorView()->points());\n ceres::CostFunction * repr_cost_function = new ReprojectionCostFunction(repr_error,\n ceres::DO_NOT_TAKE_OWNERSHIP,\n 2 * cb_views.checkerboard()->size());\n\n problem.AddResidualBlock(repr_cost_function,\n NULL,//new ceres::CauchyLoss(1.0),\n data.row(i).data(),\n data.row(i).data() + 4);\n\n problem.SetParameterization(data.row(i).data(), new ceres::QuaternionParameterization());\n }\n\n problem.SetParameterization(transform.data(), new ceres::QuaternionParameterization());\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;\n options.max_num_iterations = 20;\n options.minimizer_progress_to_stdout = true;\n options.num_threads = 8;\n\n// problem.SetParameterBlockConstant(delta);\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n\n rotation = Quaternion(transform[0], transform[1], transform[2], transform[3]);\n translation.vector() = transform.tail<3>();\n\n color_sensor_->setPose(translation * rotation);\n\n\n const int DEGREE = MathTraits::Degree;\n const int MIN_DEGREE = MathTraits::MinDegree;\n const int SIZE = DEGREE - MIN_DEGREE + 1;\n typedef MathTraits::Coefficients Coefficients;\n\n GlobalPolynomial p1(global_matrix_->model()->polynomial(0, 0));\n GlobalPolynomial p2(global_matrix_->model()->polynomial(0, 1));\n GlobalPolynomial p3(global_matrix_->model()->polynomial(1, 0));\n\n Eigen::Matrix A(SIZE, SIZE);\n Eigen::Matrix b(SIZE, 1);\n for (int i = 0; i < SIZE; ++i)\n {\n Scalar x(i + 1);\n Scalar y = p2.evaluate(x) + p3.evaluate(x) - p1.evaluate(x);\n Scalar tmp(1.0);\n for (int j = 0; j < MIN_DEGREE; ++j)\n tmp *= x;\n for (int j = 0; j < SIZE; ++j)\n {\n A(i, j) = tmp;\n tmp *= x;\n }\n b[i] = y;\n }\n\n Eigen::Matrix x = A.colPivHouseholderQr().solve(b);\n\n global_matrix_->model()->polynomial(1, 1) = x;\n\n depth_sensor_->cameraModel();\n depth_intrinsics_[0] = depth_intrinsics_[0] * delta[0];\n depth_intrinsics_[1] = depth_intrinsics_[1] * delta[1];\n depth_intrinsics_[2] = depth_intrinsics_[2] + delta[2];\n depth_intrinsics_[3] = depth_intrinsics_[3] + delta[3];\n}\n\nvoid\nCalibration::optimize ()\n{\n ROS_INFO(\"Optimizing...\\n\");\n\n if (estimate_depth_und_model_)\n {\n std::vector und_cb_views_vec;\n\n // Create locally undistorted clouds and views\n#pragma omp parallel for\n for (Size1 i = 0; i < cb_views_vec_.size(); ++i)\n {\n if (not cb_views_vec_[i])\n continue;\n\n const CheckerboardViews & cb_views = *cb_views_vec_[i];\n const DepthUndistortionEstimation::DepthData & depth_data = *depth_data_vec_[i];\n\n CheckerboardViews::Ptr und_cb_views = boost::make_shared(cb_views);\n\n RGBDData::Ptr und_data = boost::make_shared(*cb_views.data());\n und_data->setDepthData(*depth_data.undistorted_cloud_);\n\n// std::stringstream ss;\n// ss << \"/tmp/und_cloud_\" << i << \".pcd\";\n// pcl::PCDWriter writer;\n// writer.write(ss.str(), *depth_data.undistorted_cloud_);\n//\n// ss.str(\"\");\n// ss << \"/tmp/cloud_\" << i << \".pcd\";\n// writer.write(ss.str(), *depth_data.cloud_);\n\n und_cb_views->setId(cb_views.id() + \"_undistorted\");\n und_cb_views->setData(und_data);\n und_cb_views->setPlaneInliers(depth_data.estimated_plane_.indices_, depth_data.estimated_plane_.std_dev_);\n\n#pragma omp critical\n und_cb_views_vec.push_back(und_cb_views);\n }\n\n optimizeAll(und_cb_views_vec);\n }\n else\n {\n optimizeTransform(cb_views_vec_);\n }\n\n// PolynomialUndistortionMatrixIO io;\n// //io.write(*global_matrix_->model(), \"/tmp/opt_global_matrix.txt\");\n//\n// int index = 0;\n// for (Scalar i = 1.0; i < 5.5; i += 0.5, ++index)\n// {\n// Scalar max;\n// std::stringstream ss;\n// ss << \"/tmp/g_matrix_\"<< index << \".png\";\n// io.writeImageAuto(*global_matrix_->model(), i, ss.str(), max);\n// ROS_INFO_STREAM(\"Max \" << i << \": \" << max);\n// }\n\n// const Size1 size = test_vec_.size();\n//\n// for (Size1 i = 0; i < size; ++i)\n// {\n// const RGBDData::ConstPtr & test_data = test_vec_[i];\n//\n// PCLCloud3::Ptr depth_data = boost::make_shared(*test_data->depthData());\n// UndistortionPCL und;\n// und.setModel(depth_sensor_->undistortionModel());\n// und.undistort(*depth_data);\n// RGBDData::Ptr new_test_data = boost::make_shared(*test_data);\n// new_test_data->setDepthData(*depth_data);\n// new_test_data->setId(10000 + test_data->id());\n// test_vec_.push_back(new_test_data);\n//\n// }\n\n// std::ofstream transform_file;\n// transform_file.open(\"/tmp/camera_pose.yaml\");\n// geometry_msgs::Pose pose_msg;\n// tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n// transform_file << pose_msg;\n// transform_file.close();\n\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/calibration_node.cpp", "content": "#include \n\n#include \n\n#include \n\n#include \n#include \n\n#include \n\n#include \n\nusing namespace camera_info_manager;\nusing namespace calibration_msgs;\n\nnamespace calibration\n{\n\nCalibrationNode::CalibrationNode (ros::NodeHandle & node_handle)\n : node_handle_(node_handle),\n publisher_(new Publisher(node_handle)),\n has_initial_transform_(false)\n{\n checkerboards_sub_ = node_handle_.subscribe(\"checkerboard_array\", 1, &CalibrationNode::checkerboardArrayCallback, this);\n\n if (not node_handle_.getParam(\"camera_calib_url\", camera_calib_url_))\n ROS_FATAL(\"Missing \\\"camera_calib_url\\\" parameter!!\");\n\n if (not node_handle_.getParam(\"depth_camera_calib_url\", depth_camera_calib_url_))\n ROS_FATAL(\"Missing \\\"depth_camera_calib_url\\\" parameter!!\");\n\n node_handle_.param(\"camera_name\", camera_name_, std::string(\"camera\"));\n node_handle_.param(\"depth_camera_name\", depth_camera_name_, std::string(\"depth_camera\"));\n\n int undistortion_matrix_cell_size_x, undistortion_matrix_cell_size_y;\n node_handle_.param(\"undistortion_matrix/cell_size_x\", undistortion_matrix_cell_size_x, 8);\n node_handle_.param(\"undistortion_matrix/cell_size_y\", undistortion_matrix_cell_size_y, 8);\n undistortion_matrix_cell_size_.x() = undistortion_matrix_cell_size_x;\n undistortion_matrix_cell_size_.y() = undistortion_matrix_cell_size_y;\n\n int images_size_x, images_size_y;\n node_handle_.param(\"depth_image/cols\", images_size_x, 640);\n node_handle_.param(\"depth_image/rows\", images_size_y, 480);\n images_size_.x() = images_size_x;\n images_size_.y() = images_size_y;\n\n node_handle_.param(\"downsample_ratio\", downsample_ratio_, 1);\n if (downsample_ratio_ < 1)\n {\n downsample_ratio_ = 1;\n ROS_WARN(\"\\\"downsample_ratio\\\" cannot be < 1. Skipping.\");\n }\n else\n {\n images_size_ /= downsample_ratio_;\n }\n\n if (not node_handle_.getParam(\"depth_error_function\", depth_error_coeffs_))\n ROS_FATAL(\"Missing \\\"depth_error_function\\\" parameter!!\");\n else if (depth_error_coeffs_.size() != 3)\n ROS_FATAL(\"\\\"depth_error_function\\\" must be a vector of size 3!!\");\n\n if (node_handle_.hasParam(\"camera_pose\"))\n {\n Translation3 translation;\n Quaternion rotation;\n\n node_handle_.getParam(\"camera_pose/translation/x\", translation.vector().coeffRef(0));\n node_handle_.getParam(\"camera_pose/translation/y\", translation.vector().coeffRef(1));\n node_handle_.getParam(\"camera_pose/translation/z\", translation.vector().coeffRef(2));\n\n node_handle_.getParam(\"camera_pose/rotation/x\", rotation.coeffs().coeffRef(0));\n node_handle_.getParam(\"camera_pose/rotation/y\", rotation.coeffs().coeffRef(1));\n node_handle_.getParam(\"camera_pose/rotation/z\", rotation.coeffs().coeffRef(2));\n node_handle_.getParam(\"camera_pose/rotation/w\", rotation.coeffs().coeffRef(3));\n\n initial_transform_ = translation * rotation;\n has_initial_transform_ = true;\n }\n\n}\n\nbool\nCalibrationNode::initialize ()\n{\n if (not waitForMessages())\n return false;\n\n calibration_ = boost::make_shared();\n\n for (size_t i = 0; i < checkerboard_array_msg_->checkerboards.size(); ++i)\n cb_vec_.push_back(createCheckerboard(checkerboard_array_msg_->checkerboards[i], i));\n\n BaseObject::Ptr world = boost::make_shared();\n world->setFrameId(\"/world\");\n\n CameraInfoManager manager_depth(node_handle_, depth_camera_name_, depth_camera_calib_url_);\n sensor_msgs::CameraInfo depth_camera_info = manager_depth.getCameraInfo();\n depth_camera_info.binning_x = downsample_ratio_;\n depth_camera_info.binning_y = downsample_ratio_;\n KinectDepthCameraModel::Ptr depth_pinhole_model = boost::make_shared(depth_camera_info);\n\n std::cout << depth_pinhole_model->projectionMatrix() << std::endl;\n\n depth_sensor_ = boost::make_shared >();\n depth_sensor_->setFrameId(\"/depth_sensor\");\n depth_sensor_->setParent(world);\n depth_sensor_->setCameraModel(depth_pinhole_model);\n Polynomial depth_error_function(Vector3(depth_error_coeffs_[0], depth_error_coeffs_[1], depth_error_coeffs_[2]));\n depth_sensor_->setDepthErrorFunction(depth_error_function);\n\n CameraInfoManager manager(node_handle_, camera_name_, camera_calib_url_);\n PinholeCameraModel::ConstPtr pinhole_model = boost::make_shared(manager.getCameraInfo());\n\n\n color_sensor_ = boost::make_shared();\n color_sensor_->setFrameId(\"/pinhole_sensor\");\n color_sensor_->setCameraModel(pinhole_model);\n if (has_initial_transform_)\n {\n color_sensor_->setParent(depth_sensor_);\n color_sensor_->transform(initial_transform_);\n }\n\n calibration_->setCheckerboards(cb_vec_);\n calibration_->setDepthSensor(depth_sensor_);\n calibration_->setColorSensor(color_sensor_);\n\n if (not has_initial_transform_)\n calibration_->setEstimateInitialTransform(true);\n\n LocalModel::Ptr local_model = boost::make_shared(images_size_);\n LocalModel::Data::Ptr local_matrix = local_model->createMatrix(undistortion_matrix_cell_size_, LocalPolynomial::IdentityCoefficients());\n local_model->setMatrix(local_matrix);\n\n GlobalModel::Ptr global_model = boost::make_shared(images_size_);\n GlobalModel::Data::Ptr global_data = boost::make_shared(Size2(2, 2), GlobalPolynomial::IdentityCoefficients());\n global_model->setMatrix(global_data);\n\n UndistortionModel::Ptr model = boost::make_shared();\n model->setLocalModel(local_model);\n model->setGlobalModel(global_model);\n\n depth_sensor_->setUndistortionModel(model);\n\n calibration_->setLocalModel(local_model);\n calibration_->setGlobalModel(global_model);\n calibration_->initDepthUndistortionModel();\n calibration_->setPublisher(publisher_);\n calibration_->setDownSampleRatio(downsample_ratio_);\n\n return true;\n}\n\nvoid\nCalibrationNode::checkerboardArrayCallback (const CheckerboardArray::ConstPtr & msg)\n{\n checkerboard_array_msg_ = msg;\n}\n\nbool\nCalibrationNode::waitForMessages () const\n{\n ros::Rate rate(1.0);\n ros::spinOnce();\n while (ros::ok() and (not checkerboard_array_msg_))\n {\n ROS_WARN(\"Not all messages received!\");\n rate.sleep();\n ros::spinOnce();\n }\n return checkerboard_array_msg_;\n}\n\nCheckerboard::Ptr\nCalibrationNode::createCheckerboard (const CheckerboardMsg::ConstPtr & msg,\n int id)\n{\n return createCheckerboard(*msg, id);\n}\n\nCheckerboard::Ptr\nCalibrationNode::createCheckerboard (const CheckerboardMsg & msg,\n int id)\n{\n std::stringstream ss;\n ss << \"/checkerboard_\" << id;\n Checkerboard::Ptr cb = boost::make_shared(msg.cols, msg.rows, msg.cell_width, msg.cell_height);\n cb->setFrameId(ss.str());\n return cb;\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/calibration_test.cpp", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#include \n#include \n\n#include \n\n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n\n#include \n\n#include \n#include \n\nnamespace calibration\n{\n\nvoid CalibrationTest::publishData() const\n{\n if (not publisher_)\n return;\n\n publisher_->publishTF(*depth_sensor_);\n publisher_->publishTF(*color_sensor_);\n\n for (size_t i = 0; i < data_vec_.size(); i += 1)\n publisher_->publish(*data_vec_[i]);\n\n for (size_t i = 0; i < und_data_vec_.size(); i += 1)\n publisher_->publish(*und_data_vec_[i]);\n\n}\n\nPCLCloudRGB::Ptr CalibrationTest::addData(const cv::Mat & image,\n const PCLCloud3::ConstPtr & cloud)\n{\n// PCLCloud3::Ptr new_cloud = boost::make_shared();\n// std::vector remapping;\n// pcl::removeNaNFromPointCloud(*cloud, *new_cloud, remapping);\n\n PCLCloud3::Ptr new_cloud = boost::make_shared(*cloud);\n\n if (ratio_ > 1)\n {\n new_cloud = boost::make_shared();\n new_cloud->resize(cloud->size() / (ratio_ * ratio_));\n new_cloud->header = cloud->header;\n new_cloud->width = cloud->width / ratio_;\n new_cloud->height = cloud->height / ratio_;\n new_cloud->is_dense = cloud->is_dense;\n\n PCLPoint3 zero(0, 0, 0);\n float nan = std::numeric_limits::quiet_NaN();\n PCLPoint3 bad_point(nan, nan, nan);\n\n for (Size1 i = 0; i < new_cloud->height; ++i)\n {\n for (Size1 j = 0; j < new_cloud->width; ++j)\n {\n new_cloud->at(j, i) = zero;\n int count = 0;\n for (Size1 di = 0; di < ratio_; ++di)\n {\n for (Size1 dj = 0; dj < ratio_; ++dj)\n {\n const PCLPoint3 & p = cloud->at(j * ratio_ + dj, i * ratio_ + di);\n if (pcl::isFinite(p))\n {\n ++count;\n new_cloud->at(j, i).x += p.x;\n new_cloud->at(j, i).y += p.y;\n new_cloud->at(j, i).z += p.z;\n }\n }\n }\n if (count > 0)\n {\n new_cloud->at(j, i).x /= count;\n new_cloud->at(j, i).y /= count;\n new_cloud->at(j, i).z /= count;\n }\n else\n {\n new_cloud->at(j, i) = bad_point;\n new_cloud->is_dense = false;\n }\n }\n }\n }\n\n int index = data_vec_.size() + 1;\n\n cv::Mat rectified = image;\n //color_sensor_->cameraModel()->rectifyImage(image, rectified);\n\n PinholeSensor::Ptr color_sensor_original = boost::make_shared();\n color_sensor_original->setFrameId(\"/pinhole_sensor_original\");\n color_sensor_original->setCameraModel(color_sensor_->cameraModel());\n color_sensor_original->setParent(depth_sensor_);\n color_sensor_original->transform(Transform::Identity() * Translation3(0.052, 0.0, 0.0));\n\n RGBDData::Ptr data(boost::make_shared(index));\n data->setColorSensor(color_sensor_original);\n data->setDepthSensor(depth_sensor_);\n data->setColorData(rectified);\n data->setDepthData(*new_cloud);\n\n PCLCloud3::Ptr part_und_cloud = boost::make_shared(*new_cloud);\n local_matrix_->undistort(*part_und_cloud);\n\n RGBDData::Ptr part_und_data(boost::make_shared(index));\n part_und_data->setColorSensor(color_sensor_);\n part_und_data->setDepthSensor(depth_sensor_);\n part_und_data->setColorData(rectified);\n part_und_data->setDepthData(*part_und_cloud);\n\n PCLCloud3::Ptr und_cloud = boost::make_shared(*part_und_cloud);\n global_matrix_->undistort(*und_cloud);\n\n RGBDData::Ptr und_data(boost::make_shared(index + 1000));\n und_data->setColorSensor(color_sensor_);\n und_data->setDepthSensor(depth_sensor_);\n und_data->setColorData(rectified);\n und_data->setDepthData(*und_cloud);\n\n data_vec_.push_back(data);\n part_und_data_vec_.push_back(part_und_data);\n und_data_vec_.push_back(und_data);\n\n part_data_map_[und_data] = part_und_data;\n data_map_[und_data] = data;\n\n und_data->fuseData();\n return und_data->fusedData();\n\n\n // float z = 0.0f;\n // for (size_t i = 0; i < und_cloud->size(); ++i)\n // z += und_cloud->points[i].z;\n // z /= und_cloud->size();\n\n// PCLCloud3::Ptr tmp_cloud = boost::make_shared();\n// pcl::PassThrough pass;\n// pass.setInputCloud(cloud);\n// pass.setFilterFieldName(\"y\");\n// pass.setFilterLimits(-10.0f, 10.0f);\n// pass.filter(*tmp_cloud);\n\n// pcl::VoxelGrid voxel;\n// voxel.setInputCloud(tmp_cloud);\n// voxel.setLeafSize(0.05f, 10.0f, 0.05f);\n// voxel.filter(*tmp_cloud);\n\n// std::fstream fs;\n// fs.open(\"/tmp/points.txt\", std::fstream::out | std::fstream::app);\n// for (size_t i = 0; i < tmp_cloud->size(); ++i)\n// fs << tmp_cloud->points[i].x << \" \" << tmp_cloud->points[i].z << \" \" << tmp_cloud->points[i].y << std::endl;\n// fs << \"------------------------------------------------------------------\" << std::endl;\n// fs.close();\n\n// pass.setInputCloud(und_cloud);\n// pass.filter(*tmp_cloud);\n// voxel.setInputCloud(tmp_cloud);\n// voxel.filter(*tmp_cloud);\n\n// fs.open(\"/tmp/und_points.txt\", std::fstream::out | std::fstream::app);\n// for (size_t i = 0; i < tmp_cloud->size(); ++i)\n// fs << tmp_cloud->points[i].x << \" \" << tmp_cloud->points[i].z << \" \" << tmp_cloud->points[i].y << std::endl;\n// fs << \"------------------------------------------------------------------\" << std::endl;\n// fs.close();\n\n}\n\nvoid CalibrationTest::visualizeClouds() const\n{\n\n pcl::visualization::PCLVisualizer viz1(\"Test Set Visualization\");\n\n PCLCloud3::Ptr fake_cloud = boost::make_shared(640/ratio_, 480/ratio_);\n const PinholeCameraModel & camera_model = *color_sensor_->cameraModel();\n\n pcl::visualization::PointCloudColorHandlerGenericField color_handler(fake_cloud, \"z\");\n //viz1.addPointCloud(fake_cloud, color_handler);\n\n ros::Rate rate = ros::Rate(100.0);\n for (float s = 1.0f; s <= 4.5f; s += 1.5f)\n {\n\n for (size_t i = 0; i < fake_cloud->width; ++i)\n {\n for (size_t j = 0; j < fake_cloud->height; ++j)\n {\n PCLPoint3 & fake_p = fake_cloud->at(i, j);\n fake_p.x = (i - camera_model.cx()) * s / camera_model.fx();\n fake_p.y = (j - camera_model.cy()) * s / camera_model.fy();\n fake_p.z = s;\n }\n }\n\n local_matrix_->undistort(*fake_cloud);\n //global_matrix_->undistort(*fake_cloud);\n\n std::stringstream fss;\n fss << \"/tmp/fake_cloud_\" << static_cast(s * 10) << \".txt\";\n std::ofstream fs(fss.str().c_str());\n\n for (size_t j = 0; j < fake_cloud->height; ++j)\n {\n for (size_t i = 0; i < fake_cloud->width - 1; ++i)\n {\n fs << fake_cloud->at(i, j).z << \", \";\n }\n fs << fake_cloud->at(fake_cloud->width - 1, j).z << \";\" << std::endl;\n }\n\n fs.close();\n\n std::stringstream ss;\n ss << \"cloud_\" << s;\n viz1.addPointCloud(fake_cloud, color_handler, ss.str());\n\n ss.str(\"\");\n ss << s << \" m\";\n\n PCLPoint3 p = fake_cloud->points[280];\n p.y -= 0.05f;\n viz1.addText3D(ss.str(), p, 0.1, 1.0, 1.0, 1.0, ss.str());\n\n viz1.spinOnce(10, true);\n rate.sleep();\n }\n\n int ORIGINAL = 0, UNDISTORTED = 1, FINAL = 2;\n\n pcl::visualization::PCLVisualizer viz(\"Test Set Visualization\");\n viz.createViewPort(0.0, 0.0, 0.33, 1.0, ORIGINAL);\n viz.createViewPort(0.33, 0.0, 0.67, 1.0, UNDISTORTED);\n viz.createViewPort(0.67, 0.0, 1.0, 1.0, FINAL);\n\n// viz.createViewPort(0.0, 0.0, 0.5, 1.0, ORIGINAL);\n// viz.createViewPort(0.5, 0.0, 1.0, 1.0, FINAL);\n viz.addCoordinateSystem(0.5);\n\n// std::vector plane_distances(12);\n// plane_distances[0] = 1.00134;\n// plane_distances[1] = 1.20495;\n// plane_distances[2] = 1.53675;\n// plane_distances[3] = 1.86759;\n// plane_distances[4] = 2.21074;\n// plane_distances[5] = 2.54476;\n// plane_distances[6] = 2.89025;\n// plane_distances[7] = 3.23017;\n// plane_distances[8] = 3.56103;\n// plane_distances[9] = 3.89967;\n// plane_distances[10] = 4.21813;\n// plane_distances[11] = 4.57653;\n\n std::vector plane_distances;\n// plane_distances.push_back(0.943);\n// plane_distances.push_back(1.099);\n// plane_distances.push_back(1.241);\n// plane_distances.push_back(1.413);\n// plane_distances.push_back(1.615);\n// plane_distances.push_back(1.768);\n// plane_distances.push_back(1.935);\n// plane_distances.push_back(2.097);\n// plane_distances.push_back(2.261);\n// plane_distances.push_back(2.423);\n// plane_distances.push_back(2.595);\n// plane_distances.push_back(2.76);\n// plane_distances.push_back(2.906);\n// plane_distances.push_back(3.07);\n// plane_distances.push_back(3.292);\n// plane_distances.push_back(3.452);\n// plane_distances.push_back(3.612);\n// plane_distances.push_back(3.782);\n// plane_distances.push_back(4.079);\n// plane_distances.push_back(4.278);\n// plane_distances.push_back(4.455);\n// plane_distances.push_back(4.635);\n\n plane_distances.push_back(0.962);\n plane_distances.push_back(1.133);\n plane_distances.push_back(1.305);\n plane_distances.push_back(1.476);\n plane_distances.push_back(1.666);\n plane_distances.push_back(1.808);\n plane_distances.push_back(1.998);\n plane_distances.push_back(2.148);\n plane_distances.push_back(2.339);\n plane_distances.push_back(2.467);\n plane_distances.push_back(2.619);\n plane_distances.push_back(2.819);\n plane_distances.push_back(3.013);\n plane_distances.push_back(3.178);\n plane_distances.push_back(3.323);\n plane_distances.push_back(3.497);\n plane_distances.push_back(3.672);\n plane_distances.push_back(3.855);\n plane_distances.push_back(4.02);\n plane_distances.push_back(4.175);\n plane_distances.push_back(4.338);\n plane_distances.push_back(4.508);\n size_t indices[] = {0, 6, 12, 18};\n\n// plane_distances.push_back(0.927);\n// plane_distances.push_back(1.106);\n// plane_distances.push_back(1.331);\n// plane_distances.push_back(1.507);\n// plane_distances.push_back(1.661);\n// plane_distances.push_back(1.855);\n// plane_distances.push_back(2.012);\n// plane_distances.push_back(2.258);\n// plane_distances.push_back(2.457);\n// plane_distances.push_back(2.675);\n// plane_distances.push_back(2.817);\n// plane_distances.push_back(3.015);\n// plane_distances.push_back(3.179);\n// plane_distances.push_back(5);\n// plane_distances.push_back(3.531);\n// plane_distances.push_back(3.705);\n// plane_distances.push_back(3.862);\n// plane_distances.push_back(3.99);\n// plane_distances.push_back(5);\n// plane_distances.push_back(4.411);\n// size_t indices[] = {0, 6, 11, 17};\n\n for (size_t index_i = 0; index_i < 4; ++index_i)\n {\n size_t index = indices[index_i];\n\n std::stringstream ss;\n ss << \"cloud_\" << index;\n const RGBDData::ConstPtr & data = data_vec_[index];\n const RGBDData::ConstPtr & part_und_data = part_und_data_vec_[index];\n const RGBDData::ConstPtr & und_data = und_data_vec_[index];\n\n// data->fuseData();\n// part_und_data->fuseData();\n// und_data->fuseData();\n// viz.addPointCloud(data->fusedData(), ss.str(), ORIGINAL);\n// viz.addPointCloud(part_und_data->fusedData(), ss.str() + \"_und\", UNDISTORTED);\n// viz.addPointCloud(und_data->fusedData(), ss.str() + \"_final\", FINAL);\n\n// pcl::ModelCoefficients coeffs;\n// coeffs.values.resize(4);\n// coeffs.values[0] = 0.00582574;\n// coeffs.values[1] = 0.0834788;\n// coeffs.values[2] = 0.996493;\n// coeffs.values[3] = -plane_distances[index];\n\n// ss.str(\"\");\n// ss << \"plane_\" << index;\n// viz.addPlane(coeffs, ss.str());\n// viz.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.1, 0.1, ss.str());\n// viz.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, ss.str());\n\n typedef pcl::visualization::PointCloudColorHandlerGenericField ColorHandler;\n\n viz.addPointCloud(data->depthData(), ColorHandler(data->depthData(), \"y\"), ss.str(), ORIGINAL);\n viz.addPointCloud(part_und_data->depthData(), ColorHandler(part_und_data->depthData(), \"y\"), ss.str() + \"_und\", UNDISTORTED);\n viz.addPointCloud(und_data->depthData(), ColorHandler(und_data->depthData(), \"y\"), ss.str() + \"_final\", FINAL);\n\n ss.str(\"\");\n ss << index;\n PCLPoint3 p1, p2;\n// p1.x = -5; p1.y = 0; p1.z = plane_distances[index];\n// p2.x = 5; p2.y = 0; p2.z = plane_distances[index];\n p1.y = -5; p1.x = 0; p1.z = plane_distances[index];\n p2.y = 5; p2.x = 0; p2.z = plane_distances[index];\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_o\", ORIGINAL);\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_u\", UNDISTORTED);\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_f\", FINAL);\n\n }\n\n// viz.setCameraPosition(0.11384, -13.6088, 2.64823, 0.118126, 0.312233, 2.76886, 0.0, 0.0, 1.0);\n// viz.setCameraClipDistances(9.85203, 18.1708);\n viz.setCameraPosition(24.3543, 0.377151, 2.79902, 0.118126, 0.312233, 2.76886, 0.0, 1.0, 0.0);\n viz.setCameraClipDistances(19.2645, 30.751);\n viz.setCameraFieldOfView(30 * M_PI / 180);\n viz.setPosition(0, 24);\n viz.setSize(1920, 1056);\n\n\n PCLPoint3 p1, p2;\n// p1.x = -5; p1.y = 0; p1.z = 5;\n// p2.x = 5; p2.y = 0; p2.z = 5;\n p1.y = -5; p1.x = 0; p1.z = 5;\n p2.y = 5; p2.x = 0; p2.z = 5;\n viz.addLine(p1, p2, 0, 0, 0, \"5_o\", ORIGINAL);\n viz.addLine(p1, p2, 0, 0, 0, \"5_u\", UNDISTORTED);\n viz.addLine(p1, p2, 0, 0, 0, \"5_f\", FINAL);\n\n /*for (int j = 1; j <= 5; j += 2)\n {\n std::stringstream ss;\n ss << j << \"m\";\n PCLPoint3 p1, p2;\n p1.x = -5; p1.y = 0; p1.z = j;\n p2.x = 5; p2.y = 0; p2.z = j;\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_o\", ORIGINAL);\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_u\", UNDISTORTED);\n viz.addLine(p1, p2, 0, 0, 0, ss.str() + \"_f\", FINAL);\n }*/\n\n// viz.setBackgroundColor(0.08, 0.08, 0.08, UNDISTORTED);\n viz.setBackgroundColor(1.0, 1.0, 1.0, ORIGINAL);\n viz.setBackgroundColor(1.0, 1.0, 1.0, UNDISTORTED);\n viz.setBackgroundColor(1.0, 1.0, 1.0, FINAL);\n\n std::stringstream ss;\n ss << \"/home/filippo/Desktop/test/\"\n << color_sensor_->cameraModel()->tfFrame() << \"_\"\n << MathTraits::MinDegree << \"-\" << MathTraits::Degree << \"_\"\n << local_matrix_->model()->binSize().x() << \"x\" << local_matrix_->model()->binSize().y() << \".pcd\";\n\n pcl::PCDWriter writer;\n writer.write(ss.str(), *und_data_vec_[9]->depthData());\n\n// viz.addText(\"TEST SET - VISUALIZATION\", 20, 1010, 30, 1.0, 1.0, 1.0, \"test_set_text\", ORIGINAL);\n// viz.addText(\"ORIGINAL CLOUDS\", 20, 20, 30, 1.0, 1.0, 1.0, \"original_text\", ORIGINAL);\n// viz.addText(\"UNDISTORTED CLOUDS\", 20, 20, 30, 1.0, 1.0, 1.0, \"undistorted_text\", UNDISTORTED);\n// viz.addText(\"FINAL CLOUDS\", 20, 20, 30, 1.0, 1.0, 1.0, \"final_text\", FINAL);\n\n /*ros::Rate rate = ros::Rate(100.0);\n for (int i = 0; i < 500; ++i)\n {\n viz.spinOnce(10);\n rate.sleep();\n }\n\n for (size_t index = 1; index < data_vec_.size(); index += 3)\n {\n PCLCloud3::Ptr tmp_cloud = boost::make_shared(*data_vec_[index]->depthData());\n\n for (float s = 0.0f; s <= 1.0f; s += 0.02f)\n {\n\n for (size_t i = 0; i < tmp_cloud->size(); ++i)\n {\n PCLPoint3 & tmp_p = tmp_cloud->points[i];\n const PCLPoint3 & p = data_vec_[index]->depthData()->points[i];\n const PCLPoint3 & und_p = und_data_vec_[index]->depthData()->points[i];\n\n tmp_p.x = und_p.x * s + p.x * (1.0f - s);\n tmp_p.y = und_p.y * s + p.y * (1.0f - s);\n tmp_p.z = und_p.z * s + p.z * (1.0f - s);\n }\n\n RGBDData::Ptr tmp_data(boost::make_shared(index));\n tmp_data->setColorSensor(color_sensor_);\n tmp_data->setDepthSensor(depth_sensor_);\n tmp_data->setColorData(data_vec_[index]->colorData());\n tmp_data->setDepthData(*tmp_cloud);\n\n std::stringstream ss;\n ss << \"Cloud_\" << index << \"_und\";\n\n tmp_data->fuseData();\n viz.updatePointCloud(tmp_data->fusedData(), ss.str());\n viz.spinOnce(10, true);\n rate.sleep();\n }\n }*/\n /*PCLCloud3::Ptr tmp_cloud = boost::make_shared(*cloud);\n\n for (float s = 0.0f; s <= 1.0f; s += 0.02f)\n {\n\n for (size_t i = 0; i < tmp_cloud->size(); ++i)\n {\n PCLPoint3 & tmp_p = tmp_cloud->points[i];\n const PCLPoint3 & p = cloud->points[i];\n const PCLPoint3 & und_p = und_cloud->points[i];\n\n tmp_p.x = und_p.x * s + p.x * (1.0f - s);\n tmp_p.y = und_p.y * s + p.y * (1.0f - s);\n tmp_p.z = und_p.z * s + p.z * (1.0f - s);\n }\n\n RGBDData::Ptr tmp_data(boost::make_shared(index));\n tmp_data->setColorSensor(color_sensor_);\n tmp_data->setDepthSensor(depth_sensor_);\n tmp_data->setColorData(rectified);\n tmp_data->setDepthData(*tmp_cloud);\n\n tmp_data->fuseData();\n viz.updatePointCloud(tmp_data->fusedData(), \"Cloud\");\n viz.spinOnce(10, true);\n rate.sleep();\n\n }*/\n\n viz.spin();\n}\n\nvoid CalibrationTest::testPlanarityError() const\n{\n PolynomialUndistortionMatrixIO io;\n io.write(*local_matrix_->model(), \"/tmp/local_matrix.txt\");\n\n int index = 0;\n for (Scalar i = 1.0; i < 5.5; i += 0.125, ++index)\n {\n Scalar max;\n std::stringstream ss;\n ss << \"/tmp/matrix_\"<< index << \".png\";\n io.writeImageAuto(*local_matrix_->model(), i, ss.str(), max);\n ROS_INFO_STREAM(\"Max \" << i << \": \" << max);\n }\n\n\n\n std::vector cb_views_vec;\n\n CheckerboardViewsExtraction cb_extractor;\n cb_extractor.setColorSensorPose(color_sensor_->pose());\n cb_extractor.setCheckerboardVector(cb_vec_);\n cb_extractor.setInputData(und_data_vec_);\n cb_extractor.extractAll(cb_views_vec);\n\n std::map > data_map;\n\n\n for (size_t i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n RGBDData::ConstPtr und_data = cb_views.data();\n RGBDData::ConstPtr data = data_map_.at(und_data);\n\n const Cloud3 & und_points = PCLConversion::toPointMatrix(*und_data->depthData(), *cb_views.planeInliers());\n Plane und_plane = PlaneFit::fit(und_points);\n\n Scalar d_mean = 0;\n Scalar und_mean = 0;\n Scalar und_max = 0;\n Scalar und_var = 0;\n int count = 0;\n for (int p = 0; p < und_points.elements(); ++p)\n {\n if (not und_points[p].allFinite())\n continue;\n d_mean += und_points[p].z();\n Scalar d = und_plane.absDistance(und_points[p]);\n und_mean += d;\n und_var += d * d;\n if (d > und_max)\n und_max = d;\n ++count;\n }\n\n d_mean /= count;\n und_mean /= count;\n und_var /= count;\n //und_var -= und_mean * und_mean;\n\n const Cloud3 points = PCLConversion::toPointMatrix(*data->depthData(), *cb_views.planeInliers());\n Plane plane = PlaneFit::fit(points);\n\n Scalar mean = 0;\n Scalar max = 0;\n Scalar var = 0;\n for (int p = 0; p < points.elements(); ++p)\n {\n if (not points[p].allFinite())\n continue;\n Scalar d = plane.absDistance(points[p]);\n mean += d;\n var += d * d;\n if (d > max)\n max = d;\n }\n\n mean /= count;\n var /= count;\n //var -= mean * mean;\n\n data_map[d_mean].push_back(mean);\n data_map[d_mean].push_back(std::sqrt(var));\n data_map[d_mean].push_back(max);\n data_map[d_mean].push_back(und_mean);\n data_map[d_mean].push_back(std::sqrt(und_var));\n data_map[d_mean].push_back(und_max);\n data_map[d_mean].push_back(count);\n\n }\n\n// std::stringstream ss;\n// ss << \"/home/filippo/Desktop/test/\"\n// << color_sensor_->cameraModel()->tfFrame() << \"_\"\n// << MathTraits::MinDegree << \"-\" << MathTraits::Degree << \"_\"\n// << local_matrix_->model()->binSize().x() << \"x\" << local_matrix_->model()->binSize().y() << \".txt\";\n\n// std::ofstream fs(ss.str().c_str());\n\n std::cout << \"avg_distance orig_error_avg orig_error_std_dev orig_error_max und_error_avg und_error_std_dev und_error_max\" << std::endl;\n\n for (std::map >::const_iterator it = data_map.begin(); it != data_map.end(); ++it)\n std::cout << it->first << \" \"\n << it->second[0] << \" \" << it->second[1] << \" \" << it->second[2] << \" \"\n << it->second[3] << \" \" << it->second[4] << \" \" << it->second[5] << \" \"\n << static_cast(it->second[6]) << std::endl;\n\n// fs.close();\n}\n\nclass NormalError\n{\npublic:\n\n NormalError(const PinholeCameraModel::ConstPtr & camera_model,\n const Checkerboard::ConstPtr & checkerboard,\n const Cloud2 & image_corners)\n : camera_model_(camera_model),\n checkerboard_(checkerboard),\n image_corners_(image_corners)\n {\n }\n\n template \n bool operator ()(const T * const rotation,\n const T * const checkerboard_pose,\n T * residuals) const\n {\n\n typename Types::Pose checkerboard_pose_eigen = Types::Pose::Identity();\n checkerboard_pose_eigen.linear().template topLeftCorner<2, 2>() = Eigen::Rotation2D(checkerboard_pose[0]).matrix();\n checkerboard_pose_eigen.translation() = Eigen::Map >(&checkerboard_pose[1]);\n\n Eigen::Matrix new_base;\n T unit_xyz[] = {0.0, 0.0, 1.0, 0.0, 0.0};\n ceres::QuaternionRotatePoint(rotation, &unit_xyz[2], new_base.col(0).data());\n ceres::QuaternionRotatePoint(rotation, &unit_xyz[1], new_base.col(1).data());\n ceres::QuaternionRotatePoint(rotation, &unit_xyz[0], new_base.col(2).data());\n\n checkerboard_pose_eigen = new_base * checkerboard_pose_eigen;\n\n typename Types::Cloud3 cb_corners(checkerboard_->corners().size());\n cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast();\n typename Types::Cloud2 reprojected_corners = camera_model_->project3dToPixel(cb_corners);\n\n for (Size1 i = 0; i < cb_corners.elements(); ++i)\n {\n typename Types::Vector2 diff = (reprojected_corners[i] - image_corners_[i].cast());\n residuals[2 * i + 0] = diff.x();\n residuals[2 * i + 1] = diff.y();\n }\n\n return true;\n }\n\nprivate:\n\n const PinholeCameraModel::ConstPtr & camera_model_;\n const Checkerboard::ConstPtr & checkerboard_;\n const Cloud2 & image_corners_;\n\n};\n\nvoid CalibrationTest::testCheckerboardError() const\n{\n std::vector cb_views_vec;\n\n CheckerboardViewsExtraction cb_extractor;\n cb_extractor.setColorSensorPose(color_sensor_->pose());\n cb_extractor.setCheckerboardVector(cb_vec_);\n cb_extractor.setInputData(und_data_vec_);\n cb_extractor.extractAll(cb_views_vec);\n\n ceres::Problem problem;\n Eigen::Matrix data(cb_views_vec.size(), 4);\n\n Eigen::Matrix3d rot_matrix;\n rot_matrix.col(2) = cb_views_vec[0]->colorCheckerboard()->plane().normal();\n rot_matrix.col(0) = (Vector3::UnitX() - Vector3::UnitX().dot(rot_matrix.col(2)) * rot_matrix.col(2)).normalized();\n rot_matrix.col(1) = rot_matrix.col(2).cross(rot_matrix.col(0));\n\n Quaternion q(rot_matrix);\n Eigen::Matrix rotation(q.w(), q.x(), q.y(), q.z());\n\n for (Size1 i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n data.row(i)[0] = std::acos((cb_views.colorCheckerboard()->corners()(1, 0) - cb_views.colorCheckerboard()->corners()(0, 0)).normalized().x());\n data.row(i).tail<3>() = rot_matrix.inverse() * cb_views.colorCheckerboard()->pose().translation();\n\n NormalError * error = new NormalError(color_sensor_->cameraModel(),\n cb_views.checkerboard(),\n cb_views.colorView()->points());\n\n typedef ceres::NumericDiffCostFunction NormalCostFunction;\n\n ceres::CostFunction * cost_function = new NormalCostFunction(error,\n ceres::DO_NOT_TAKE_OWNERSHIP,\n 0 + 2 * cb_views.checkerboard()->size());\n problem.AddResidualBlock(cost_function, NULL, rotation.data(), data.row(i).data());\n }\n\n problem.SetParameterization(rotation.data(), new ceres::QuaternionParameterization());\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_QR;\n options.max_num_iterations = 50;\n options.minimizer_progress_to_stdout = true;\n options.num_threads = 8;\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n\n std::vector plane_vec;\n for (size_t i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n\n Pose checkerboard_pose_eigen = Pose::Identity();\n checkerboard_pose_eigen.linear().template topLeftCorner<2, 2>() = Eigen::Rotation2Dd(data.row(i)[0]).matrix();\n checkerboard_pose_eigen.translation() = data.row(i).tail<3>();\n\n Quaternion q(rotation[0], rotation[1], rotation[2], rotation[3]);\n Transform rot_matrix = q.matrix() * Transform::Identity();\n\n Checkerboard cb(*cb_views.checkerboard());\n cb.transform(rot_matrix * checkerboard_pose_eigen);\n std::cout << cb.plane().normal().dot(cb.corners().container().rowwise().mean()) << std::endl;\n cb.transform(color_sensor_->pose());\n //std::cout << cb.plane().normal().transpose() << \": \" << cb.plane().normal().dot(cb.corners().container().rowwise().mean()) << std::endl;\n\n// AngleAxis rotation;\n// rotation.angle() = data.row(i).head<3>().norm();\n// rotation.axis() = data.row(i).head<3>().normalized();\n// Translation3 translation(data.row(i).tail<3>());\n// cb.transform(translation * rotation);\n plane_vec.push_back(cb.plane());\n }\n\n Vector3 n_z = plane_vec[0].normal();\n Vector3 n_x = (Vector3::UnitX() - Vector3::UnitX().dot(n_z) * n_z).normalized();\n Vector3 n_y = n_z.cross(n_x);\n\n\n for (size_t i = 0; i < cb_views_vec.size(); ++i)\n {\n const CheckerboardViews & cb_views = *cb_views_vec[i];\n RGBDData::ConstPtr und_data = cb_views.data();\n RGBDData::ConstPtr part_und_data = part_data_map_.at(und_data);\n RGBDData::ConstPtr data = data_map_.at(und_data);\n Plane plane = plane_vec[i]; //cb_views.colorCheckerboard()->plane();\n //plane.transform(color_sensor_->pose());\n\n const Cloud3 & und_points = PCLConversion::toPointMatrix(*und_data->depthData(), *cb_views.planeInliers());\n PCLCloud3::Ptr tmp_und_cloud = boost::make_shared(und_points.size().x(), und_points.size().y());\n\n// PointPlaneExtraction plane_extractor;\n// plane_extractor.setInputCloud(und_data->depthData());\n// const pcl::PointCloud::ConstPtr und_normals_pcl = plane_extractor.cloudNormals();\n// Cloud3 und_normals(Size2(cb_views.planeInliers()->size(), 1));\n// for (Size1 j = 0; j < cb_views.planeInliers()->size(); ++j)\n// {\n// const pcl::Normal & p = und_normals_pcl->points[(*cb_views.planeInliers())[j]];\n// und_normals[j] << p.normal_x, p.normal_y, p.normal_z;\n// }\n\n\n Point3 und_d_mean(0.0, 0.0, 0.0);\n Scalar und_mean = 0;\n Scalar und_mean_abs = 0;\n Scalar und_mean2 = 0;\n// Vector3 und_angle_mean = Vector3::Zero();\n// Vector3 und_angle_mean2 = Vector3::Zero();\n int count = 0, angle_count = 0;\n for (int p = 0; p < und_points.elements(); ++p)\n {\n if (not und_points[p].allFinite())\n continue;\n und_d_mean += und_points[p];\n Scalar d = plane.signedDistance(und_points[p]);\n und_mean += d;\n und_mean_abs += std::abs(d);\n und_mean2 += d * d;\n ++count;\n tmp_und_cloud->points[p].x = und_points[p].x();\n tmp_und_cloud->points[p].y = und_points[p].y();\n tmp_und_cloud->points[p].z = d;\n\n// if (not und_normals[p].allFinite())\n// continue;\n// Scalar a = std::acos(und_normals[p].normalized().dot(n_z));\n// if (a > M_PI_2)\n// a = M_PI - a;\n// und_angle_mean[2] += a;\n// und_angle_mean2[2] += a * a;\n\n// a = std::acos(und_normals[p].normalized().dot(n_x));\n// und_angle_mean[0] += a;\n// und_angle_mean2[0] += a * a;\n\n// a = std::acos(und_normals[p].normalized().dot(n_y));\n// und_angle_mean[1] += a;\n// und_angle_mean2[1] += a * a;\n\n// ++angle_count;\n }\n\n und_d_mean /= count;\n und_mean /= count;\n und_mean_abs /= count;\n und_mean2 /= count;\n// und_angle_mean /= angle_count;\n// und_angle_mean2 /= angle_count;\n\n Scalar und_std_dev = std::sqrt(und_mean2 - und_mean * und_mean);\n// Vector3 und_angle_std_dev = (und_angle_mean2 - und_angle_mean.cwiseAbs2()).cwiseSqrt();\n\n const Cloud3 points = PCLConversion::toPointMatrix(*data->depthData(), *cb_views.planeInliers());\n PCLCloud3::Ptr tmp_cloud = boost::make_shared(points.size().x(), points.size().y());\n\n// plane_extractor.setInputCloud(data->depthData());\n// const pcl::PointCloud::ConstPtr normals_pcl = plane_extractor.cloudNormals();\n// Cloud3 normals(Size2(cb_views.planeInliers()->size(), 1));\n// for (Size1 j = 0; j < cb_views.planeInliers()->size(); ++j)\n// {\n// const pcl::Normal & p = normals_pcl->points[(*cb_views.planeInliers())[j]];\n// normals[j] << p.normal_x, p.normal_y, p.normal_z;\n// }\n\n Point3 d_mean(0.0, 0.0, 0.0);\n Scalar mean = 0;\n Scalar mean2 = 0;\n// Vector3 angle_mean = Vector3::Zero();\n// Vector3 angle_mean2 = Vector3::Zero();\n// angle_count = 0;\n for (Size1 p = 0; p < points.elements(); ++p)\n {\n if (not points[p].allFinite())\n continue;\n d_mean += points[p];\n Scalar d = plane.signedDistance(points[p]);\n mean += d;\n mean2 += d * d;\n tmp_cloud->points[p].x = points[p].x();\n tmp_cloud->points[p].y = points[p].y();\n tmp_cloud->points[p].z = d;\n\n// if (not und_normals[p].allFinite())\n// continue;\n// Scalar a = std::acos(normals[p].normalized().dot(n_z));\n// if (a > M_PI_2)\n// a = M_PI - a;\n// angle_mean[2] += a;\n// angle_mean2[2] += a * a;\n\n// a = std::acos(normals[p].normalized().dot(n_x));\n// angle_mean[0] += a;\n// angle_mean2[0] += a * a;\n\n// a = std::acos(normals[p].normalized().dot(n_y));\n// angle_mean[1] += a;\n// angle_mean2[1] += a * a;\n\n// ++angle_count;\n\n }\n\n d_mean /= count;\n mean /= count;\n mean2 /= count;\n// angle_mean /= angle_count;\n// angle_mean2 /= angle_count;\n\n Scalar std_dev = std::sqrt(mean2 - mean * mean);\n// Vector3 angle_std_dev = (angle_mean2 - angle_mean.cwiseAbs2()).cwiseSqrt();\n\n const Cloud3 part_points = PCLConversion::toPointMatrix(*part_und_data->depthData(), *cb_views.planeInliers());\n\n Point3 part_d_mean(0.0, 0.0, 0.0);\n Scalar part_mean = 0;\n Scalar part_mean2 = 0;\n for (int p = 0; p < part_points.elements(); ++p)\n {\n if (not part_points[p].allFinite())\n continue;\n part_d_mean += part_points[p];\n Scalar d = plane.signedDistance(part_points[p]);\n part_mean += d;\n part_mean2 += d * d;\n }\n\n part_d_mean /= count;\n part_mean /= count;\n part_mean2 /= count;\n Scalar part_std_dev = std::sqrt(part_mean2 - part_mean * part_mean);\n\n Plane und_fitted_plane = PlaneFit::fit(und_points);\n if (und_fitted_plane.offset() < 0)\n und_fitted_plane.coeffs() *= -1.0;\n Scalar und_angle_x = std::acos(und_fitted_plane.normal().dot(n_x));\n Scalar und_angle_y = std::acos(und_fitted_plane.normal().dot(n_y));\n\n Plane fitted_plane = PlaneFit::fit(points);\n if (fitted_plane.offset() < 0)\n fitted_plane.coeffs() *= -1.0;\n Scalar angle_x = std::acos(fitted_plane.normal().dot(n_x));\n Scalar angle_y = std::acos(fitted_plane.normal().dot(n_y));\n\n Plane part_fitted_plane = PlaneFit::fit(part_points);\n if (part_fitted_plane.offset() < 0)\n part_fitted_plane.coeffs() *= -1.0;\n Scalar part_angle_x = std::acos(part_fitted_plane.normal().dot(n_x));\n Scalar part_angle_y = std::acos(part_fitted_plane.normal().dot(n_y));\n\n std::cout << \"Original: \" << plane.normal().dot(d_mean) << \", \" << mean << \", \" << std_dev << \", \"\n << 180 * angle_x / M_PI << \", \" << 180 * angle_y / M_PI << \"; \"\n << \"Final: \" << plane.normal().dot(und_d_mean) << \", \"<< und_mean << \", \" << und_std_dev << \", \"\n << 180 * und_angle_x / M_PI << \", \" << 180 * und_angle_y / M_PI << \"; \"\n << \"Undistorted: \" << plane.normal().dot(part_d_mean) << \", \"<< part_mean << \", \" << part_std_dev << \", \"\n << 180 * part_angle_x / M_PI << \", \" << 180 * part_angle_y / M_PI << \"; \" << std::endl;\n\n\n\n pcl::VoxelGrid voxel;\n voxel.setInputCloud(tmp_cloud);\n voxel.setLeafSize(0.04f, 10.0f, 0.03f);\n voxel.filter(*tmp_cloud);\n\n std::fstream fs;\n std::stringstream ss;\n ss << \"/tmp/points_\" << i << \".txt\";\n fs.open(ss.str().c_str(), std::fstream::out);\n for (size_t j = 0; j < tmp_cloud->size(); ++j)\n fs << tmp_cloud->points[j].x << \" \" << tmp_cloud->points[j].z << std::endl;\n fs.close();\n\n voxel.setInputCloud(tmp_und_cloud);\n voxel.filter(*tmp_und_cloud);\n\n ss.str(\"\");\n ss << \"/tmp/und_points_\" << i << \".txt\";\n fs.open(ss.str().c_str(), std::fstream::out);\n for (size_t j = 0; j < tmp_und_cloud->size(); ++j)\n fs << tmp_und_cloud->points[j].x << \" \" << tmp_und_cloud->points[j].z << std::endl;\n fs.close();\n\n\n\n\n\n }\n\n}\n\nboost::shared_ptr >\nCalibrationTest::extractPlaneFromCloud (const PCLCloud3::Ptr & cloud,\n const Cloud2 & depth_corners) const\n{\n float min_x = depth_corners[0].x();\n float max_x = min_x;\n float min_y = depth_corners[0].y();\n float max_y = min_y;\n for (int i = 1; i < 4; ++i)\n {\n min_x = std::min(min_x, depth_corners[i].x());\n max_x = std::max(max_x, depth_corners[i].x());\n min_y = std::min(min_y, depth_corners[i].y());\n max_y = std::max(max_y, depth_corners[i].y());\n }\n\n min_x = std::max(min_x, 0.0f);\n max_x = std::min(max_x, static_cast(cloud->width));\n min_y = std::max(min_y, 0.0f);\n max_y = std::min(max_y, static_cast(cloud->height));\n\n Vector2 lines[4];\n\n for (int i = 0; i < 4; ++i)\n lines[i] = depth_corners[(i + 1) % 4] - depth_corners[i];\n\n boost::shared_ptr > indices = boost::make_shared >();\n indices->reserve(static_cast((max_x - min_x) * (max_y - min_y)));\n\n for (float y = std::ceil(min_y); y < max_y; ++y)\n {\n for (float x = std::ceil(min_x); x < max_x; ++x)\n {\n bool ok = true;\n\n Vector2 vec = Vector2(x, y) - depth_corners[0];\n int sign;\n if (lines[0].x() * vec.y() - lines[0].y() * vec.x() >= 0)\n sign = 1;\n else\n sign = -1;\n\n for (int i = 1; i < 4; ++i)\n {\n Vector2 vec = Vector2(x, y) - depth_corners[i];\n if (sign * (lines[i].x() * vec.y() - lines[i].y() * vec.x()) < 0)\n {\n ok = false;\n break;\n }\n }\n if (ok)\n indices->push_back(static_cast(y * cloud->width + x));\n }\n }\n\n return indices;\n\n}\n\n\nvoid CalibrationTest::testCube() const\n{\n Checkerboard checkerboard(4, 4, 0.095, 0.095);\n\n std::vector > data_vec;\n\n for (size_t i = 0; i < und_data_vec_.size(); ++i)\n {\n#ifndef UNCALIBRATED\n const RGBDData & data = *und_data_vec_.at(i);\n#else\n const RGBDData & data = *data_vec_.at(i);\n#endif\n cv::Mat tmp_image = data.colorData().clone();\n //cv::Mat tmp_image_rect = tmp_image;\n\n /*cv::undistort(tmp_image, tmp_image_rect,\n color_sensor_->cameraModel()->intrinsicMatrix(),\n color_sensor_->cameraModel()->distortionCoeffs(),\n color_sensor_->cameraModel()->intrinsicMatrix());*/\n\n //color_sensor_->cameraModel()->rectifyImage(tmp_image, tmp_image_rect);\n AutomaticCheckerboardFinder finder;\n\n std::vector > planes;\n std::vector > points;\n std::vector > depth_planes;\n std::vector > depth_points;\n\n\n //pcl::visualization::PCLVisualizer viz(\"VIZ\");\n\n for (int c = 0; c < 3; ++c)\n {\n finder.setImage(tmp_image);\n Cloud2 corners;\n bool found = finder.find(checkerboard, corners);\n\n Pose pose = color_sensor_->estimatePose(corners, checkerboard.corners());\n Checkerboard tmp_checkerboard(6, 6, 0.095, 0.095);\n tmp_checkerboard.transform(pose * Translation3(-0.095, -0.095, 0.0));\n Cloud2 tmp_corners = color_sensor_->cameraModel()->project3dToPixel2(tmp_checkerboard.corners());\n\n if (found)\n {\n cv::vector points;\n /*points.push_back(cv::Point(corners[0].x(), corners[0].y()));\n points.push_back(cv::Point(corners[3].x(), corners[3].y()));\n points.push_back(cv::Point(corners[15].x(), corners[15].y()));\n points.push_back(cv::Point(corners[12].x(), corners[12].y()));*/\n\n points.push_back(cv::Point(tmp_corners[0].x(), tmp_corners[0].y()));\n points.push_back(cv::Point(tmp_corners[5].x(), tmp_corners[5].y()));\n points.push_back(cv::Point(tmp_corners[35].x(), tmp_corners[35].y()));\n points.push_back(cv::Point(tmp_corners[30].x(), tmp_corners[30].y()));\n\n cv::fillConvexPoly(tmp_image, points, cv::Scalar(c == 0 ? 128 : 0, c == 1 ? 128 : 0, c == 2 ? 128 : 0));\n\n for (int corner = 0; corner < tmp_corners.elements(); ++corner)\n cv::circle(tmp_image, cv::Point2f(tmp_corners[corner].x(), tmp_corners[corner].y()), 2, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));\n //cv::fillConvexPoly(tmp_image_rect, points, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));\n }\n else\n {\n ROS_WARN(\"Pattern not found\");\n InteractiveCheckerboardFinder i_finder;\n i_finder.setImage(tmp_image);\n\n found = i_finder.find(checkerboard, corners, true);\n\n Pose pose = color_sensor_->estimatePose(corners, checkerboard.corners());\n tmp_checkerboard = Checkerboard(6, 6, 0.095, 0.095);\n tmp_checkerboard.transform(pose * Translation3(-0.095, -0.095, 0.0));\n Cloud2 tmp_corners = color_sensor_->cameraModel()->project3dToPixel2(tmp_checkerboard.corners());\n\n if (found)\n {\n cv::vector points;\n\n points.push_back(cv::Point(tmp_corners[0].x(), tmp_corners[0].y()));\n points.push_back(cv::Point(tmp_corners[5].x(), tmp_corners[5].y()));\n points.push_back(cv::Point(tmp_corners[35].x(), tmp_corners[35].y()));\n points.push_back(cv::Point(tmp_corners[30].x(), tmp_corners[30].y()));\n\n cv::fillConvexPoly(tmp_image, points, cv::Scalar(c == 0 ? 128 : 0, c == 1 ? 128 : 0, c == 2 ? 128 : 0));\n\n for (int corner = 0; corner < tmp_corners.elements(); ++corner)\n cv::circle(tmp_image, cv::Point2f(tmp_corners[corner].x(), tmp_corners[corner].y()), 2, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));\n //cv::fillConvexPoly(tmp_image_rect, points, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));\n }\n }\n\n //cv::drawChessboardCorners(tmp_image, cv::Size(checkerboard.cols(), checkerboard.rows()), corners, found);\n\n Cloud3 depth_corners(Size2(1, 4));\n depth_corners(0, 0) = tmp_checkerboard(0, 0);\n depth_corners(0, 1) = tmp_checkerboard(0, 5);\n depth_corners(0, 2) = tmp_checkerboard(5, 5);\n depth_corners(0, 3) = tmp_checkerboard(5, 0);\n depth_corners.transform(color_sensor_->pose());\n\n//#ifdef HERRERA\n std::vector in(4);\n in[0] = cv::Point3f(depth_corners(0, 0).x(), depth_corners(0, 0).y(), depth_corners(0, 0).z());\n in[1] = cv::Point3f(depth_corners(0, 1).x(), depth_corners(0, 1).y(), depth_corners(0, 1).z());\n in[2] = cv::Point3f(depth_corners(0, 2).x(), depth_corners(0, 2).y(), depth_corners(0, 2).z());\n in[3] = cv::Point3f(depth_corners(0, 3).x(), depth_corners(0, 3).y(), depth_corners(0, 3).z());\n\n std::vector out;\n//#ifdef HERRERA\n cv::projectPoints(in, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n depth_sensor_->cameraModel()->intrinsicMatrix(), depth_sensor_->cameraModel()->distortionCoeffs(), out);\n//#else\n// cv::projectPoints(in, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n// depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);\n//#endif\n Cloud2 image_corners(Size2(1, 4));\n image_corners(0, 0) = Point2(out[0].x, out[0].y);\n image_corners(0, 1) = Point2(out[1].x, out[1].y);\n image_corners(0, 2) = Point2(out[2].x, out[2].y);\n image_corners(0, 3) = Point2(out[3].x, out[3].y);\n//#else\n// Cloud2 image_corners = depth_sensor_->cameraModel()->project3dToPixel(depth_corners);\n//#endif\n\n boost::shared_ptr > indices = extractPlaneFromCloud(data.depthData(), image_corners);\n\n data.fuseData();\n\n boost::shared_ptr > new_indices = boost::shared_ptr >(new std::vector());\n new_indices->reserve(indices->size());\n for (size_t i_i = 0; i_i < indices->size(); ++i_i)\n {\n const pcl::PointXYZRGB & point = (*data.fusedData())[(*indices)[i_i]];\n if (point.r + point.g + point.b > 300)\n new_indices->push_back((*indices)[i_i]);\n }\n\n pcl::StatisticalOutlierRemoval sor;\n sor.setInputCloud(data.fusedData());\n sor.setIndices(new_indices);\n sor.setMeanK(10);\n sor.setStddevMulThresh(1.0);\n sor.filter(*new_indices);\n\n pcl::ExtractIndices extract;\n extract.setInputCloud(data.depthData());\n extract.setIndices(new_indices);\n extract.setNegative(false);\n pcl::PointCloud::Ptr cloud_p (new pcl::PointCloud);\n extract.filter (*cloud_p);\n\n /*std::stringstream ss;\n ss << \"cloud_\" << c;\n viz.addPointCloud(cloud_p, ss.str());*/\n\n Cloud3 checkerboards_3d = PCLConversion::toPointMatrix(*data.depthData(), *new_indices);\n\n Eigen::Vector4f centroid;\n pcl::compute3DCentroid(*data.depthData(), *new_indices, centroid);\n Point3 point = color_sensor_->pose().inverse() * centroid.cast().head<3>();\n\n checkerboards_3d.transform(color_sensor_->pose().inverse());\n\n Cloud2 proj = color_sensor_->cameraModel()->project3dToPixel2(checkerboards_3d);\n\n for (Size1 i = 0; i < proj.elements(); ++i)\n cv::circle(tmp_image, cv::Point(proj[i].x(), proj[i].y()), 1, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));\n\n PlaneFit plane_fit;\n Plane plane = plane_fit.fit(checkerboards_3d);\n\n /*pcl::ModelCoefficients cf;\n cf.values.resize(4);\n cf.values[0] = plane.coeffs()[0];\n cf.values[1] = plane.coeffs()[1];\n cf.values[2] = plane.coeffs()[2];\n cf.values[3] = plane.coeffs()[3];\n viz.addPlane(cf, ss.str() + \"_plane\");\n\n pcl::ModelCoefficients cf2;\n cf2.values.resize(4);\n cf2.values[0] = tmp_checkerboard.plane().coeffs()[0];\n cf2.values[1] = tmp_checkerboard.plane().coeffs()[1];\n cf2.values[2] = tmp_checkerboard.plane().coeffs()[2];\n cf2.values[3] = tmp_checkerboard.plane().coeffs()[3];\n viz.addPlane(cf2, ss.str() + \"_cb\");*/\n\n point = plane.projection(point);\n\n if (found)\n {\n planes.push_back(tmp_checkerboard.plane());\n points.push_back(tmp_checkerboard.corners()[0]);\n depth_planes.push_back(plane);\n depth_points.push_back(point);\n }\n\n }\n //viz.spin();\n\n Point3 rgb_point, depth_point;\n\n\n data_vec.push_back(std::vector(20));\n\n {\n Eigen::Matrix N;\n N.col(0) = planes.at(0).normal();\n N.col(1) = planes.at(1).normal();\n N.col(2) = planes.at(2).normal();\n\n rgb_point = 1.0 / N.determinant() * (points.at(0).dot(N.col(0)) * N.col(1).cross(N.col(2)) +\n points.at(1).dot(N.col(1)) * N.col(2).cross(N.col(0)) +\n points.at(2).dot(N.col(2)) * N.col(0).cross(N.col(1)));\n\n //Point2 pixel = color_sensor_->cameraModel()->project3dToPixel(rgb_point);\n //cv::circle(tmp_image_rect, cv::Point(pixel.x(), pixel.y()), 3, cv::Scalar(0, 0, 0));\n\n std::vector points(1);\n points[0] = cv::Point3f(rgb_point.x(), rgb_point.y(), rgb_point.z());\n\n std::vector out(1);\n //cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n // depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);\n\n//#ifdef HERRERA\n cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(), out);\n/*#else\n cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n color_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);\n#endif*/\n\n cv::circle(tmp_image, out[0], 3, cv::Scalar(0, 0, 255));\n\n Scalar a0 = 90.0 - 180.0 / M_PI * std::acos(N.col(0).dot(N.col(1)));\n Scalar a1 = 90.0 - 180.0 / M_PI * std::acos(N.col(1).dot(N.col(2)));\n Scalar a2 = 90.0 - 180.0 / M_PI * std::acos(N.col(2).dot(N.col(0)));\n ROS_INFO_STREAM(\"Pixel RGB: \" << out[0].x << \", \" << out[0].y);\n ROS_INFO_STREAM(\"Angular error RGB: \" << a0 << \", \" << a1 << \", \" << a2);\n\n data_vec.back().at(0) = out[0].x;\n data_vec.back().at(1) = out[0].y;\n data_vec.back().at(2) = a0;\n data_vec.back().at(3) = a1;\n data_vec.back().at(4) = a2;\n\n }\n\n {\n Eigen::Matrix N;\n N.col(0) = depth_planes.at(0).normal();\n N.col(1) = depth_planes.at(1).normal();\n N.col(2) = depth_planes.at(2).normal();\n\n depth_point = 1.0 / N.determinant() * (depth_points.at(0).dot(N.col(0)) * N.col(1).cross(N.col(2)) +\n depth_points.at(1).dot(N.col(1)) * N.col(2).cross(N.col(0)) +\n depth_points.at(2).dot(N.col(2)) * N.col(0).cross(N.col(1)));\n\n //Point2 pixel = color_sensor_->cameraModel()->project3dToPixel(depth_point);\n //cv::circle(tmp_image_rect, cv::Point(pixel.x(), pixel.y()), 3, cv::Scalar(0, 0, 255));\n\n std::vector points(1);\n points[0] = cv::Point3f(depth_point.x(), depth_point.y(), depth_point.z());\n\n std::vector out(1);\n //cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n // depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);\n\n//#ifdef HERRERA\n cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(), out);\n/*#else\n cv::projectPoints(points, cv::Mat_::zeros(3, 1), cv::Mat_::zeros(3, 1),\n color_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);\n#endif*/\n\n cv::circle(tmp_image, out[0], 3, cv::Scalar(0, 0, 0));\n\n Scalar a0 = 90.0 - 180.0 / M_PI * std::acos(N.col(0).dot(N.col(1)));\n Scalar a1 = 90.0 - 180.0 / M_PI * std::acos(N.col(1).dot(N.col(2)));\n Scalar a2 = 90.0 - 180.0 / M_PI * std::acos(N.col(2).dot(N.col(0)));\n ROS_INFO_STREAM(\"Pixel DEPTH: \" << out[0].x << \", \" << out[0].y);\n ROS_INFO_STREAM(\"Angular error DEPTH: \" << a0 << \", \" << a1 << \", \" << a2);\n\n data_vec.back().at(5) = out[0].x;\n data_vec.back().at(6) = out[0].y;\n data_vec.back().at(7) = a0;\n data_vec.back().at(8) = a1;\n data_vec.back().at(9) = a2;\n\n }\n\n Scalar a0 = 180.0 / M_PI * std::acos(planes.at(0).normal().dot(depth_planes.at(0).normal()));\n Scalar a1 = 180.0 / M_PI * std::acos(planes.at(1).normal().dot(depth_planes.at(1).normal()));\n Scalar a2 = 180.0 / M_PI * std::acos(planes.at(2).normal().dot(depth_planes.at(2).normal()));\n\n ROS_INFO_STREAM(\"Point RGB: \" << rgb_point.x() << \", \" << rgb_point.y() << \", \" << rgb_point.z());\n ROS_INFO_STREAM(\"Point DEPTH: \" << depth_point.x() << \", \" << depth_point.y() << \", \" << depth_point.z());\n ROS_INFO_STREAM(\"Depth error: \" << (rgb_point - depth_point).norm());\n //ROS_INFO_STREAM(rgb_point.transpose() << \" -- \" << depth_point.transpose() << \" => \" << (rgb_point - depth_point).norm());\n ROS_INFO_STREAM(\"Angular error: \" << (a0 > 90 ? 180 - a0 : a0) << \", \" << (a1 > 90 ? 180 - a1 : a1) << \", \" << (a2 > 90 ? 180 - a2 : a2));\n\n ROS_INFO_STREAM(planes.at(0).normal().transpose() << \" -- \" << planes.at(1).normal().transpose() << \" -- \" << planes.at(2).normal().transpose() << \" -- \");\n\n cv::imshow(\"Image\", tmp_image);\n cv::waitKey();\n\n data_vec.back().at(10) = rgb_point.x();\n data_vec.back().at(11) = rgb_point.y();\n data_vec.back().at(12) = rgb_point.z();\n data_vec.back().at(13) = depth_point.x();\n data_vec.back().at(14) = depth_point.y();\n data_vec.back().at(15) = depth_point.z();\n data_vec.back().at(16) = (rgb_point - depth_point).norm();\n data_vec.back().at(17) = (a0 > 90 ? 180 - a0 : a0);\n data_vec.back().at(18) = (a1 > 90 ? 180 - a1 : a1);\n data_vec.back().at(19) = (a2 > 90 ? 180 - a2 : a2);\n\n }\n\n for (size_t i = 0; i < data_vec.size(); ++i)\n for (size_t j = 0; j < data_vec[i].size(); ++j)\n std::cout << data_vec[i][j] << (j == data_vec[i].size() - 1 ? \"\\n\" : \", \");\n\n\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/checkerboard_views.cpp", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#include \n#include \n\n#include \n\nnamespace calibration\n{\n\nvoid CheckerboardViews::setColorView(const PinholeView::ConstPtr & color_view)\n{\n color_view_ = boost::make_shared >(*color_view);\n // color_checkerboard_ = boost::make_shared(*color_view_);\n\n std::stringstream ss;\n ss << checkerboard_->frameId() << \"_\" << id_;\n\n color_checkerboard_ = boost::make_shared(*checkerboard_);\n color_checkerboard_->setParent(data_->colorSensor());\n color_checkerboard_->setFrameId(ss.str());\n color_checkerboard_->transform(color_view_->sensor()->estimatePose(color_view_->points(), checkerboard_->corners()));\n}\n\nvoid CheckerboardViews::setImageCorners(const Cloud2 & image_corners)\n{\n PinholeView::Ptr color_view = boost::make_shared >();\n color_view->setId(id_);\n color_view->setPoints(image_corners);\n color_view->setSensor(data_->colorSensor());\n color_view->setObject(checkerboard_);\n\n setColorView(color_view);\n}\n\nvoid CheckerboardViews::setPlaneInliers(const pcl::IndicesConstPtr & plane_inliers,\n Scalar inliers_std_dev)\n{\n plane_inliers_ = plane_inliers;\n\n depth_view_ = boost::make_shared >();\n depth_view_->setId(id_);\n depth_view_->setData(data_->depthData());\n depth_view_->setPoints(*plane_inliers);\n depth_view_->setSensor(data_->depthSensor());\n depth_view_->setObject(checkerboard_);\n\n depth_plane_ = boost::make_shared();\n depth_plane_->setParent(data_->depthSensor());\n\n std::stringstream ss;\n ss << checkerboard_->frameId() << \"_plane_\" << id_;\n depth_plane_->setFrameId(ss.str());\n\n depth_plane_->setPlane(PlaneFit::robustFit(PCLConversion::toPointMatrix(*depth_view_->data(), depth_view_->points()), inliers_std_dev));\n //depth_plane_->setPlane(PlaneFit::fit(depth_view_->points()));\n}\n\nvoid CheckerboardViews::setPlaneInliers(const PlaneInfo & plane_info)\n{\n plane_inliers_ = plane_info.indices_;\n\n depth_view_ = boost::make_shared >();\n depth_view_->setId(id_);\n depth_view_->setData(data_->depthData());\n// depth_view_->setPoints(PCLConversion::toPointMatrix(*data_->depthData(), *plane_info.indices_));\n depth_view_->setPoints(*plane_info.indices_);\n depth_view_->setSensor(data_->depthSensor());\n depth_view_->setObject(checkerboard_);\n\n depth_plane_ = boost::make_shared();\n depth_plane_->setParent(data_->depthSensor());\n\n std::stringstream ss;\n ss << checkerboard_->frameId() << \"_plane_\" << id_;\n depth_plane_->setFrameId(ss.str());\n\n depth_plane_->setPlane(plane_info.plane_);\n}\n\nvoid CheckerboardViews::draw(cv::Mat & image) const\n{\n cv::Size pattern_size(color_checkerboard_->rows(), color_checkerboard_->cols());\n std::vector corners;\n for (int i = 0; i < color_view_->points().size().prod(); ++i)\n corners.push_back(cv::Point2f(color_view_->points()[i][0], color_view_->points()[i][1]));\n\n cv::drawChessboardCorners(image, pattern_size, cv::Mat(corners), true);\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/checkerboard_views_extractor.cpp", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#include \n\n#include \n#include \n#include \n\nnamespace calibration\n{\n\nSize1 CheckerboardViewsExtraction::extract(const RGBDData::ConstPtr & data,\n std::vector & cb_views_vec,\n bool interactive,\n bool force) const\n{\n Size1 added = 0;\n\n cv::Mat image = data->colorData().clone();\n AutomaticCheckerboardFinder finder;\n finder.setImage(image);\n\n PointPlaneExtraction::Ptr plane_extractor;\n\n if (not interactive)\n plane_extractor = boost::make_shared >();\n else\n plane_extractor = boost::make_shared >();\n\n plane_extractor->setInputCloud(data->depthData());\n\n for (Size1 c = 0; c < cb_vec_.size(); ++c)\n {\n const Checkerboard::ConstPtr & cb = cb_vec_[c];\n\n plane_extractor->setRadius(std::min(cb->width(), cb->height()) / 1.5);\n\n std::stringstream ss;\n ss << \"rgbd_cb_\" << data->id() << \"_\" << c;\n CheckerboardViews::Ptr cb_views(boost::make_shared(ss.str()));\n cb_views->setData(data);\n cb_views->setCheckerboard(cb);\n\n // 1. Extract corners\n\n Cloud2 image_corners(cb->corners().size());\n if (not finder.find(*cb, image_corners))\n {\n if (force)\n {\n InteractiveCheckerboardFinder finder2;\n finder2.setImage(image);\n if (not finder2.find(*cb, image_corners))\n continue;\n }\n else\n continue;\n }\n\n cb_views->setImageCorners(image_corners);\n\n if (not cb_constraint_->isValid(*cb_views->colorCheckerboard()))\n continue;\n\n // 2. Extract plane\n\n if (not only_images_)\n {\n PlaneInfo plane_info;\n\n if (interactive)\n {\n cb_views->draw(image);\n }\n else\n {\n Point3 center = color_sensor_pose_ * cb_views->colorCheckerboard()->center();\n PCLPoint3 p;\n p.x = center[0];\n p.y = center[1];\n p.z = center[2];\n plane_extractor->setPoint(p);\n }\n\n if (not plane_extractor->extract(plane_info))\n continue;\n\n cb_views->setPlaneInliers(plane_info.indices_, plane_info.std_dev_);\n\n if (not plane_constraint_->isValid(*cb_views->depthPlane()))\n continue;\n\n }\n\n#pragma omp critical\n cb_views_vec.push_back(cb_views);\n\n ++added;\n }\n\n return added;\n\n}\n\nSize1 CheckerboardViewsExtraction::extract(std::vector & cb_views_vec,\n bool interactive) const\n{\n return extract(data_, cb_views_vec, interactive, true);\n}\n\nSize1 CheckerboardViewsExtraction::extractAll(std::vector & cb_views_vec,\n bool interactive) const\n{\n Size1 added = 0;\n\n#pragma omp parallel for\n for (Size1 i = 0; i < data_vec_.size(); ++i)\n {\n Size1 n = extract(data_vec_[i], cb_views_vec, interactive, force_);\n#pragma omp atomic\n added += n;\n }\n\n return added;\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/data_collection_node.cpp", "content": "#include \n#include \n\n#include \n#include \n#include \n#include \n\n//#include \n//#include \n#include \n#include \n#include \n\n#include \n#include \n\n#include \n\n#include \n\n#include \n#include \n\n#include \n\nusing namespace camera_info_manager;\nusing namespace calibration_msgs;\n\nusing rgbd_calibration::Acquisition;\n\nnamespace calibration\n{\n\nclass DataCollectionNode\n{\npublic:\n\n enum\n {\n SAVE_NONE = 0,\n SAVE_IMAGE = 1,\n SAVE_DEPTH_IMAGE = 2,\n SAVE_POINT_CLOUD = 4,\n SAVE_IMAGE_CAMERA_INFO = 8,\n SAVE_DEPTH_CAMERA_INFO = 16,\n SAVE_ALL = 31\n };\n\n enum DepthType\n {\n DEPTH_FLOAT32,\n DEPTH_UINT16\n };\n\n DataCollectionNode(ros::NodeHandle & node_handle);\n\n virtual\n ~DataCollectionNode();\n\n bool\n initialize();\n\n static Checkerboard::Ptr\n createCheckerboard(const CheckerboardMsg::ConstPtr & msg,\n int id);\n\n static Checkerboard::Ptr\n createCheckerboard(const CheckerboardMsg & msg,\n int id);\n\nprotected:\n\n void\n save(const sensor_msgs::CameraInfo::ConstPtr & camera_info,\n const std::string & file_name);\n\n void\n save(const pcl::PCLPointCloud2::ConstPtr & cloud,\n const std::string & file_name);\n\n void\n save(const cv::Mat & image,\n const std::string & file_name);\n\n void\n saveDepth(const cv::Mat & depth_image,\n const std::string & file_name);\n\n void\n pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr & msg);\n\n void\n imageCallback(const sensor_msgs::Image::ConstPtr & msg);\n\n void\n depthImageCallback(const sensor_msgs::Image::ConstPtr & msg);\n\n void\n cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg);\n\n void\n depthCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg);\n\n void\n actionCallback(const Acquisition::ConstPtr & msg);\n\n void\n checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg);\n\n bool\n waitForMessages();\n\n /* variables */\n\n ros::NodeHandle node_handle_;\n image_transport::ImageTransport image_transport_;\n\n ros::Subscriber cloud_sub_;\n image_transport::Subscriber image_sub_;\n image_transport::Subscriber depth_image_sub_;\n ros::Subscriber camera_info_sub_;\n ros::Subscriber depth_camera_info_sub_;\n\n ros::Subscriber omnicamera_info_sub_;\n ros::Subscriber action_sub_;\n\n pcl::PCLPointCloud2::Ptr cloud_msg_;\n sensor_msgs::Image::ConstPtr image_msg_;\n sensor_msgs::Image::ConstPtr depth_image_msg_;\n sensor_msgs::CameraInfo::ConstPtr camera_info_msg_;\n sensor_msgs::CameraInfo::ConstPtr depth_camera_info_msg_;\n\n// TODO find another way to get checkerboards\n ros::Subscriber checkerboards_sub_;\n calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;\n\n std::vector cb_vec_;\n std::vector > last_corners_;\n std::vector > last_saved_corners_;\n\n int starting_index_;\n int file_index_;\n\n std::string save_folder_;\n std::string image_extension_;\n std::string image_filename_;\n std::string depth_filename_;\n std::string cloud_filename_;\n\n bool search_checkerboard_;\n\n int save_flags_;\n DepthType depth_type_;\n\n};\n\nDataCollectionNode::DataCollectionNode(ros::NodeHandle & node_handle)\n : node_handle_(node_handle),\n image_transport_(node_handle),\n search_checkerboard_(false)\n{\n checkerboards_sub_ = node_handle_.subscribe(\"checkerboard_array\", 1, &DataCollectionNode::checkerboardArrayCallback, this);\n\n action_sub_ = node_handle.subscribe(\"action\", 1, &DataCollectionNode::actionCallback, this);\n\n node_handle_.param(\"starting_index\", starting_index_, 1);\n if (not node_handle_.getParam(\"save_folder\", save_folder_))\n ROS_FATAL_STREAM(\"[\" << ros::this_node::getName() << \"] Missing folder!!\");\n\n if (save_folder_.at(save_folder_.size() - 1) != '/')\n save_folder_.append(\"/\");\n\n file_index_ = starting_index_;\n\n node_handle_.param(\"image_extension\", image_extension_, std::string(\"png\"));\n node_handle_.param(\"image_filename\", image_filename_, std::string(\"image_\"));\n node_handle_.param(\"depth_filename\", depth_filename_, std::string(\"depth_\"));\n node_handle_.param(\"cloud_filename\", cloud_filename_, std::string(\"cloud_\"));\n\n node_handle_.param(\"search_checkerboard\", search_checkerboard_, false);\n\n bool save_image, save_image_camera_info;\n node_handle_.param(\"save_image\", save_image, false);\n node_handle_.param(\"save_image_camera_info\", save_image_camera_info, false);\n\n bool save_depth_image, save_depth_camera_info;\n node_handle_.param(\"save_depth_image\", save_depth_image, false);\n node_handle_.param(\"save_depth_camera_info\", save_depth_camera_info, false);\n\n bool save_point_cloud;\n node_handle_.param(\"save_point_cloud\", save_point_cloud, false);\n\n save_flags_ = 0;\n save_flags_ |= save_image ? SAVE_IMAGE : 0;\n save_flags_ |= save_image_camera_info ? SAVE_IMAGE_CAMERA_INFO : 0;\n save_flags_ |= save_depth_image ? SAVE_DEPTH_IMAGE : 0;\n save_flags_ |= save_depth_camera_info ? SAVE_DEPTH_CAMERA_INFO : 0;\n save_flags_ |= save_point_cloud ? SAVE_POINT_CLOUD : 0;\n\n if (save_flags_ & SAVE_POINT_CLOUD)\n cloud_sub_ = node_handle.subscribe(\"point_cloud\", 1, &DataCollectionNode::pointCloudCallback, this);\n\n if (save_flags_ & SAVE_IMAGE)\n image_sub_ = image_transport_.subscribe(\"image\", 1, &DataCollectionNode::imageCallback, this);\n\n if (save_flags_ & SAVE_DEPTH_IMAGE)\n depth_image_sub_ = image_transport_.subscribe(\"depth_image\", 1, &DataCollectionNode::depthImageCallback, this);\n\n if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)\n camera_info_sub_ = node_handle.subscribe(\"camera_info\", 1, &DataCollectionNode::cameraInfoCallback, this);\n\n if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)\n depth_camera_info_sub_ = node_handle.subscribe(\"depth_camera_info\", 1, &DataCollectionNode::depthCameraInfoCallback, this);\n\n std::string depth_type_s;\n node_handle_.param(\"depth_type\", depth_type_s, std::string(\"float32\"));\n if (depth_type_s == std::string(\"float32\"))\n depth_type_ = DEPTH_FLOAT32;\n else if (depth_type_s == std::string(\"uint16\"))\n depth_type_ = DEPTH_UINT16;\n else\n ROS_FATAL_STREAM(\"[\" << ros::this_node::getName() << \"] Wrong \\\"depth_type\\\" parameter. Use \\\"float32\\\" or \\\"uint16\\\".\");\n\n}\n\nDataCollectionNode::~DataCollectionNode()\n{\n // Do nothing\n}\n\nbool DataCollectionNode::initialize()\n{\n if (not waitForMessages())\n return false;\n\n ros::spinOnce();\n\n if (search_checkerboard_)\n {\n for (size_t i = 0; i < checkerboard_array_msg_->checkerboards.size(); ++i)\n cb_vec_.push_back(createCheckerboard(checkerboard_array_msg_->checkerboards[i], i));\n }\n\n return true;\n}\n\nvoid DataCollectionNode::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr & msg)\n{\n cloud_msg_ = pcl::PCLPointCloud2::Ptr(new pcl::PCLPointCloud2());\n pcl_conversions::toPCL(*msg, *cloud_msg_);\n}\n\nvoid DataCollectionNode::imageCallback(const sensor_msgs::Image::ConstPtr & msg)\n{\n image_msg_ = msg;\n}\n\nvoid DataCollectionNode::depthImageCallback(const sensor_msgs::Image::ConstPtr & msg)\n{\n depth_image_msg_ = msg;\n}\n\nvoid DataCollectionNode::cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg)\n{\n camera_info_msg_ = msg;\n}\n\nvoid DataCollectionNode::depthCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg)\n{\n depth_camera_info_msg_ = msg;\n}\n\nvoid DataCollectionNode::actionCallback(const Acquisition::ConstPtr & msg)\n{\n\n try\n {\n std::stringstream info_file_name;\n info_file_name << save_folder_ << \"info.yaml\";\n std::stringstream file_index_ss;\n file_index_ss << std::setw(4) << std::setfill('0') << file_index_;\n\n std::ofstream file;\n file.open(info_file_name.str().c_str(), file_index_ == 1 ? std::ios_base::out : std::ios_base::out | std::ios_base::app);\n if (file_index_ == 1)\n file << \"data:\" << std::endl;\n file << \" - id: \" << file_index_ss.str() << std::endl;\n file << \" timestamp_image: \" << std::setprecision(19) << image_msg_->header.stamp.toSec() << std::setprecision(6) << std::endl;\n file << \" timestamp_depth: \" << std::setprecision(19) << depth_image_msg_->header.stamp.toSec() << std::setprecision(6) << std::endl;\n //file << \" distance: \" << msg->distance << std::endl;\n //file << \" info: \\\"\" << msg->info << \"\\\"\" << std::endl;\n file.close();\n\n if (file_index_ == 1)\n {\n if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)\n {\n std::stringstream camera_info_file_name;\n camera_info_file_name << save_folder_ << image_filename_ << \"camera_info.yaml\";\n save(camera_info_msg_, camera_info_file_name.str());\n }\n\n if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)\n {\n std::stringstream depth_camera_info_file_name;\n depth_camera_info_file_name << save_folder_ << depth_filename_ << \"camera_info.yaml\";\n save(depth_camera_info_msg_, depth_camera_info_file_name.str());\n }\n }\n\n if (save_flags_ & SAVE_POINT_CLOUD)\n {\n std::stringstream cloud_file_name;\n cloud_file_name << save_folder_ << cloud_filename_ << file_index_ss.str() << \".pcd\";\n save(cloud_msg_, cloud_file_name.str());\n }\n\n if (save_flags_ & SAVE_IMAGE)\n {\n cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(image_msg_);\n std::stringstream image_file_name;\n image_file_name << save_folder_ << image_filename_ << file_index_ss.str() << \".\" << image_extension_;\n save(image_ptr->image, image_file_name.str());\n }\n\n if (save_flags_ & SAVE_DEPTH_IMAGE)\n {\n cv_bridge::CvImage::Ptr depth_image_ptr;\n if (depth_type_ == DEPTH_FLOAT32)\n depth_image_ptr = cv_bridge::toCvCopy(depth_image_msg_, sensor_msgs::image_encodings::TYPE_32FC1);\n else if (depth_type_ == DEPTH_UINT16)\n depth_image_ptr = cv_bridge::toCvCopy(depth_image_msg_, sensor_msgs::image_encodings::TYPE_16UC1);\n std::stringstream depth_file_name;\n depth_file_name << save_folder_ << depth_filename_ << file_index_ss.str() << \".\" << image_extension_;\n saveDepth(depth_image_ptr->image, depth_file_name.str());\n }\n\n ROS_INFO_STREAM_THROTTLE(1, \"[\" << ros::this_node::getName() << \"] \" << file_index_ss.str() << \" saved\");\n file_index_++;\n\n }\n catch (cv_bridge::Exception & ex)\n {\n ROS_ERROR_STREAM(\"[\" << ros::this_node::getName() << \"] cv_bridge exception: \" << ex.what());\n }\n\n\n}\n\nvoid DataCollectionNode::save(const sensor_msgs::CameraInfo::ConstPtr & camera_info,\n const std::string & file_name)\n{\n image_geometry::PinholeCameraModel model;\n model.fromCameraInfo(camera_info);\n\n std::ofstream file;\n file.open(file_name.c_str());\n\n file << \"frame_id: \" << camera_info_msg_->header.frame_id << std::endl;\n file << \"height: \" << camera_info_msg_->height << std::endl;\n file << \"width: \" << camera_info_msg_->width << std::endl;\n file << \"distortion_model: \" << camera_info_msg_->distortion_model << std::endl;\n file << \"D: \" << model.distortionCoeffs() << std::endl;\n file << \"K: \" << model.intrinsicMatrix().reshape<1, 9>() << std::endl;\n file << \"R: \" << model.rotationMatrix().reshape<1, 9>() << std::endl;\n file << \"P: \" << model.projectionMatrix().reshape<1, 12>() << std::endl;\n file << \"binning_x: \" << camera_info_msg_->binning_x << std::endl;\n file << \"binning_y: \" << camera_info_msg_->binning_y << std::endl;\n file << \"roi:\" << std::endl;\n file << \" x_offset: \" << camera_info_msg_->roi.x_offset << std::endl;\n file << \" y_offset: \" << camera_info_msg_->roi.y_offset << std::endl;\n file << \" height: \" << camera_info_msg_->roi.height << std::endl;\n file << \" width: \" << camera_info_msg_->roi.width << std::endl;\n file << \" do_rectify: \" << (camera_info_msg_->roi.do_rectify ? \"True\" : \"False\") << std::endl;\n\n file.close();\n}\n\nvoid DataCollectionNode::checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg)\n{\n checkerboard_array_msg_ = msg;\n}\n\nbool DataCollectionNode::waitForMessages()\n{\n ROS_INFO_STREAM(\"[\" << ros::this_node::getName() << \"] Waiting for sensors...\");\n bool ret = true;\n\n if (search_checkerboard_)\n ret = ret and ros::topic::waitForMessage(\"checkerboard_array\", node_handle_);\n\n if (save_flags_ & SAVE_IMAGE)\n ret = ret and ros::topic::waitForMessage(\"image\", node_handle_);\n\n if (save_flags_ & SAVE_POINT_CLOUD)\n ret = ret and ros::topic::waitForMessage(\"point_cloud\", node_handle_);\n\n if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)\n ret = ret and ros::topic::waitForMessage(\"camera_info\", node_handle_);\n\n if (save_flags_ & SAVE_DEPTH_IMAGE)\n ret = ret and ros::topic::waitForMessage(\"depth_image\", node_handle_);\n\n if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)\n ret = ret and ros::topic::waitForMessage(\"depth_camera_info\", node_handle_);\n\n if (ret)\n ROS_INFO_STREAM(\"[\" << ros::this_node::getName() << \"] All sensors connected\");\n else\n ROS_ERROR_STREAM(\"[\" << ros::this_node::getName() << \"] Not all sensors connected!\");\n\n return ret;\n}\n\nvoid DataCollectionNode::save(const pcl::PCLPointCloud2::ConstPtr & cloud,\n const std::string & file_name)\n{\n pcl::PCDWriter pcd_writer;\n pcd_writer.writeBinary(file_name, *cloud);\n}\n\nvoid DataCollectionNode::save(const cv::Mat & image,\n const std::string & file_name)\n{\n cv::imwrite(file_name, image);\n}\n\nvoid DataCollectionNode::saveDepth(const cv::Mat & depth_image,\n const std::string & file_name)\n{\n if (depth_type_ == DEPTH_FLOAT32)\n {\n cv::Mat depth_image_16;\n depth_image.convertTo(depth_image_16, CV_16UC1, 1000);\n cv::imwrite(file_name, depth_image_16);\n }\n else if (depth_type_ == DEPTH_UINT16)\n {\n cv::imwrite(file_name, depth_image);\n }\n else\n {\n // Do nothing\n }\n}\n\nCheckerboard::Ptr DataCollectionNode::createCheckerboard(const CheckerboardMsg::ConstPtr & msg,\n int id)\n{\n return createCheckerboard(*msg, id);\n}\n\nCheckerboard::Ptr DataCollectionNode::createCheckerboard(const CheckerboardMsg & msg,\n int id)\n{\n std::stringstream ss;\n ss << \"/checkerboard_\" << id;\n Checkerboard::Ptr cb = boost::make_shared(msg.cols, msg.rows, msg.cell_width, msg.cell_height);\n cb->setFrameId(ss.str());\n return cb;\n}\n\n} /* namespace calibration */\n\nusing namespace calibration;\n\nint main(int argc,\n char ** argv)\n{\n ros::init(argc, argv, \"rgbd_data_collector\");\n ros::NodeHandle node_handle(\"~\");\n\n try\n {\n DataCollectionNode collector_node(node_handle);\n if (not collector_node.initialize())\n return 0;\n ros::spin();\n }\n catch (std::runtime_error & error)\n {\n ROS_FATAL(\"Calibration error: %s\", error.what());\n return 1;\n }\n\n return 0;\n}\n" }, { "path": "src/rgbd_calibration/depth_undistortion_estimation.cpp", "content": "/*\n * Copyright (c) 2013-2014, Filippo Basso \n *\n * All rights reserved.\n *\n * Redistribution and use in source and binary forms, with or without\n * modification, are permitted provided that the following conditions are met:\n * 1. Redistributions of source code must retain the above copyright\n * notice, this list of conditions and the following disclaimer.\n * 2. Redistributions in binary form must reproduce the above copyright\n * notice, this list of conditions and the following disclaimer in the\n * documentation and/or other materials provided with the distribution.\n * 3. Neither the name of the copyright holder(s) nor the\n * names of its contributors may be used to endorse or promote products\n * derived from this software without specific prior written permission.\n *\n * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\n * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\n * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\n * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY\n * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\n * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\n * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\n * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\n * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n */\n\n#include \n#include \n#include \n\n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#define RGBD_INFO(id, msg) ROS_INFO_STREAM(\"RGBD \" << id << \": \" << msg)\n#define RGBD_WARN(id, msg) ROS_WARN_STREAM(\"RGBD \" << id << \": \" << msg)\n\nnamespace calibration\n{\n\nbool DepthUndistortionEstimation::extractPlane(const Checkerboard & color_cb,\n const PCLCloud3::ConstPtr & cloud,\n const Point3 & center,\n PlaneInfo & plane_info)\n{\n Scalar radius = std::min(color_cb.width(), color_cb.height()) / 1.5; // TODO Add parameter\n PointPlaneExtraction plane_extractor;\n plane_extractor.setInputCloud(cloud);\n plane_extractor.setRadius(radius);\n\n bool plane_extracted = false;\n\n int r[] = {0, 1, 2}; // TODO Add parameter\n int k[] = {0, 1, -1, 2, -2}; // TODO Add parameter\n for (Size1 i = 0; i < 5 and not plane_extracted; ++i)\n {\n for (Size1 j = 0; j < 3 and not plane_extracted; ++j)\n {\n plane_extractor.setRadius((1 + r[j]) * radius);\n plane_extractor.setPoint(PCLPoint3(center.x(), center.y(), center.z() + (1 + r[j]) * radius * k[i]));\n plane_extracted = plane_extractor.extract(plane_info);\n }\n }\n\n return plane_extracted;\n}\n\nvoid DepthUndistortionEstimation::estimateLocalModel()\n{\n std::sort(data_vec_.begin(), data_vec_.end(), OrderByDistance());\n\n for (Size1 i = 0; i < data_vec_.size(); i += max_threads_)\n {\n#pragma omp parallel for schedule(static, 1)\n for (Size1 th = 0; th < max_threads_; ++th)\n {\n if (i + th >= data_vec_.size())\n continue;\n\n const DepthData & data = *data_vec_[i + th];\n const Checkerboard & gt_cb = *data.checkerboard_;\n const PCLCloud3 & cloud = *data.cloud_;\n\n // Estimate center\n Point3 und_color_cb_center = gt_cb.center();\n#pragma omp critical\n {\n InverseGlobalMatrixEigen inverse_global(inverse_global_fit_->model());\n inverse_global.undistort(0, 0, und_color_cb_center);\n }\n\n// RGBD_INFO(data.id_, \"Transformed z: \" << gt_cb.center().z() << \" -> \" << und_color_cb_center.z());\n\n // Undistort cloud\n PCLCloud3::Ptr und_cloud = boost::make_shared(cloud);\n#pragma omp critical\n {\n LocalMatrixPCL local(local_fit_->model());\n local.undistort(*und_cloud);\n }\n\n // Extract plane from undistorted cloud\n PlaneInfo plane_info;\n if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))\n {\n// RGBD_INFO(data.id_, \"Plane extracted!!\");\n plane_info_map_[data_vec_[i + th]] = plane_info;\n\n if ((i + th) % 10 == 0)\n {\n PCLCloud3::Ptr tmp_und_cloud = boost::make_shared(*und_cloud, *plane_info.indices_);\n PCLCloud3::Ptr tmp_cloud = boost::make_shared(cloud, *plane_info.indices_);\n\n std::stringstream ss;\n pcl::PCDWriter writer;\n ss << \"/tmp/tmp_und_cloud_\" << i + th << \".pcd\";\n writer.write(ss.str(), *und_cloud);\n\n ss.str(\"\");\n ss << \"/tmp/tmp_und_cloud_\" << i + th << \"_plane.pcd\";\n writer.write(ss.str(), *tmp_und_cloud);\n\n ss.str(\"\");\n ss << \"/tmp/tmp_cloud_\" << i + th << \".pcd\";\n writer.write(ss.str(), cloud);\n\n ss.str(\"\");\n ss << \"/tmp/tmp_cloud_\" << i + th << \"_plane.pcd\";\n writer.write(ss.str(), *tmp_cloud);\n\n PlaneInfo tmp_plane_info;\n if (extractPlane(gt_cb, data.cloud_, und_color_cb_center, tmp_plane_info))\n {\n PCLCloud3::Ptr tmp_cloud_2 = boost::make_shared(cloud, *tmp_plane_info.indices_);\n\n ss.str(\"\");\n ss << \"/tmp/tmp_cloud_\" << i + th << \"_plane_2.pcd\";\n writer.write(ss.str(), *tmp_cloud_2);\n std::cout << plane_info.std_dev_ << \" -- \" << tmp_plane_info.std_dev_ << std::endl;\n }\n }\n\n// Plane fitted_plane = PlaneFit::robustFit(PCLConversion::toPointMatrix(*und_cloud, *plane_info.indices_),\n// plane_info.std_dev_);\n\n std::vector indices;// = *plane_info.indices_;\n indices.reserve(plane_info.indices_->size());\n int w = und_cloud->width;\n int h = und_cloud->height;\n for (size_t j = 0; j < plane_info.indices_->size(); ++j)\n {\n int r = (*plane_info.indices_)[j] / w;\n int c = (*plane_info.indices_)[j] % w;\n if ((r - h/2)*(r - h/2) + (c - w/2)*(c - w/2) < (h/2)*(h/2))\n indices.push_back((*plane_info.indices_)[j]);\n }\n Plane fitted_plane = PlaneFit::fit(PCLConversion::toPointMatrix(cloud, indices)/*, plane_info.std_dev_*/);\n\n plane_info_map_[data_vec_[i + th]].plane_ = fitted_plane;\n\n\n#pragma omp critical\n {\n local_fit_->accumulateCloud(cloud, *plane_info.indices_);\n local_fit_->addAccumulatedPoints(fitted_plane);\n for (Size1 c = 0; c < gt_cb.corners().elements(); ++c)\n {\n const Point3 & corner = gt_cb.corners()[c];\n inverse_global_fit_->addPoint(0, 0, corner, fitted_plane);\n }\n if (i + th > 20)\n inverse_global_fit_->update();\n\n }\n\n Line line(Vector3::Zero(), gt_cb.center().normalized());\n RGBD_INFO(data.id_, \"Transformed z: \" << gt_cb.center().z() << \" -> \" << und_color_cb_center.z()\n << \" (Real z: \" << line.intersectionPoint(fitted_plane).z() << \")\");\n\n// Scalar angle = RAD2DEG(std::acos(plane_info.equation_.normal().dot(gt_cb.plane().normal())));\n// RGBD_INFO(data.id(), \"Angle: \" << angle);\n\n }\n else\n RGBD_WARN(data.id_, \"Plane not extracted!!\");\n\n }\n\n local_fit_->update();\n }\n\n}\n\nvoid DepthUndistortionEstimation::estimateLocalModelReverse()\n{\n //std::reverse(data_vec_.begin(), data_vec_.end());\n\n local_fit_->reset();\n\n for (Size1 i = 0; i < data_vec_.size(); i += max_threads_)\n {\n#pragma omp parallel for schedule(static, 1)\n for (Size1 th = 0; th < max_threads_; ++th)\n {\n if (i + th >= data_vec_.size())\n continue;\n\n const DepthData & data = *data_vec_[i + th];\n const Checkerboard & gt_cb = *data.checkerboard_;\n const PCLCloud3 & cloud = *data.cloud_;\n\n // Estimate center\n Point3 und_color_cb_center = gt_cb.center();\n#pragma omp critical\n {\n InverseGlobalMatrixEigen inverse_global(inverse_global_fit_->model());\n inverse_global.undistort(0, 0, und_color_cb_center);\n }\n\n// RGBD_INFO(data.id_, \"Transformed z: \" << gt_cb.center().z() << \" -> \" << und_color_cb_center.z());\n\n // Undistort cloud\n PCLCloud3::Ptr und_cloud = boost::make_shared(cloud);\n#pragma omp critical\n {\n LocalMatrixPCL local(local_fit_->model());\n local.undistort(*und_cloud);\n }\n\n // Extract plane from undistorted cloud\n PlaneInfo plane_info;\n if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))\n {\n// RGBD_INFO(data.id_, \"Plane extracted!!\");\n\n// Plane fitted_plane = PlaneFit::robustFit(PCLConversion::toPointMatrix(*und_cloud, *plane_info.indices_),\n// plane_info.std_dev_);\n\n boost::shared_ptr > indices = boost::make_shared >();// = *plane_info.indices_;\n indices->reserve(plane_info.indices_->size());\n int w = und_cloud->width;\n int h = und_cloud->height;\n for (size_t j = 0; j < plane_info.indices_->size(); ++j)\n {\n int r = (*plane_info.indices_)[j] / w;\n int c = (*plane_info.indices_)[j] % w;\n if ((r - h/2)*(r - h/2) + (c - w/2)*(c - w/2) < (h/2)*(h/2))\n indices->push_back((*plane_info.indices_)[j]);\n }\n Plane fitted_plane = PlaneFit::fit(PCLConversion::toPointMatrix(cloud, *indices)/*, plane_info.std_dev_*/);\n\n\n boost::shared_ptr > old_indices;\n#pragma omp critical\n {\n old_indices = plane_info_map_[data_vec_[i + th]].indices_;\n }\n indices->clear();\n std::set_union(old_indices->begin(), old_indices->end(), plane_info.indices_->begin(), plane_info.indices_->end(), std::back_inserter(*indices));\n\n#pragma omp critical\n {\n local_fit_->accumulateCloud(cloud, *indices);\n local_fit_->addAccumulatedPoints(fitted_plane);\n plane_info_map_[data_vec_[i + th]].indices_ = indices;\n }\n\n Line line(Vector3::Zero(), gt_cb.center().normalized());\n RGBD_INFO(data.id_, \"Transformed z: \" << gt_cb.center().z() << \" -> \" << und_color_cb_center.z()\n << \" (Real z: \" << line.intersectionPoint(fitted_plane).z() << \")\");\n\n }\n else\n RGBD_WARN(data.id_, \"Plane not extracted!!\");\n\n }\n\n }\n local_fit_->update();\n //std::reverse(data_vec_.begin(), data_vec_.end());\n\n}\n\nclass LocalModelError\n{\npublic:\n\n LocalModelError(const PCLCloud3::ConstPtr & cloud,\n const Plane & plane,\n const LocalModel::Ptr & local_model,\n const Polynomial & depth_error_function)\n : cloud_(cloud), plane_(plane), depth_error_function_(depth_error_function)\n {\n local_matrix_.setModel(local_model);\n }\n\n void addIndex(int x_index, int y_index)\n {\n indices_.push_back(std::make_pair(x_index, y_index));\n }\n\n void createCloudWithIndices()\n {\n part_cloud_.height = 1;\n part_cloud_.width = indices_.size();\n part_cloud_.points.resize(indices_.size());\n for (Size1 i = 0; i < indices_.size(); ++i)\n {\n int index = indices_[i].first + cloud_->width * indices_[i].second;\n part_cloud_.points[i] = cloud_->points[index];\n }\n }\n\n int size() const\n {\n return indices_.size();\n }\n\n bool operator ()(const double * const local_poly_1_data,\n const double * const local_poly_2_data,\n const double * const local_poly_3_data,\n const double * const local_poly_4_data,\n double * residuals) const\n {\n PCLCloud3 tmp_cloud = part_cloud_;\n\n Eigen::Map residuals_map(residuals, 3, size());\n Eigen::MatrixX4d polynomials(MathTraits::Size, 4);\n polynomials.col(0) = Eigen::Map::Size, 1> >(local_poly_1_data);\n polynomials.col(1) = Eigen::Map::Size, 1> >(local_poly_2_data);\n polynomials.col(2) = Eigen::Map::Size, 1> >(local_poly_3_data);\n polynomials.col(3) = Eigen::Map::Size, 1> >(local_poly_4_data);\n\n for (Size1 i = 0; i < indices_.size(); ++i)\n {\n const std::pair & index = indices_[i];\n\n std::vector lt_data = local_matrix_.model()->lookupTable(index.first, index.second);\n\n double tmp_depth = 0.0;\n double depth = tmp_cloud.points[i].z;\n for (Size1 j = 0; j < lt_data.size(); ++j)\n tmp_depth += lt_data[j].weight_ * LocalPolynomial::evaluate(polynomials.col(j), depth);\n depth = tmp_depth;\n double k = depth / tmp_cloud.points[i].z;\n\n Point3 p(tmp_cloud.points[i].x * k, tmp_cloud.points[i].y * k, depth);\n Line line(Vector3::Zero(), p.normalized());\n// residuals[i] = (p - line.intersectionPoint(plane_)).norm() / ceres::poly_eval(depth_error_function_.coefficients(), p.z());\n residuals_map.col(i) = (p - line.intersectionPoint(plane_)) / ceres::poly_eval(depth_error_function_.coefficients(), p.z());\n }\n\n return true;\n }\n\nprivate:\n\n const PCLCloud3::ConstPtr cloud_;\n const Plane plane_;\n LocalMatrixPCL local_matrix_;\n const Polynomial depth_error_function_;\n std::vector > indices_;\n PCLCloud3 part_cloud_;\n\n};\n\ntypedef ceres::NumericDiffCostFunction::Size,\nMathTraits::Size, MathTraits::Size, MathTraits::Size> LocalCostFunction;\n\nvoid DepthUndistortionEstimation::optimizeLocalModel(const Polynomial & depth_error_function)\n{\n std::vector all_errors_vec;\n ceres::Problem problem;\n\n for (Size1 i = 0; i < data_vec_.size(); ++i)\n {\n const DepthData::ConstPtr & data = data_vec_[i];\n const std::vector & indices = *plane_info_map_[data].indices_;\n\n int delta_x = data->cloud_->width / local_model_->binSize().x();\n int delta_y = data->cloud_->height / local_model_->binSize().y();\n\n std::vector error_vec;\n\n for (Size1 j = 0; j < delta_y * delta_x; ++j)\n {\n error_vec.push_back(new LocalModelError(data->cloud_, plane_info_map_[data].plane_, local_model_, depth_error_function));\n all_errors_vec.push_back(error_vec.back());\n }\n\n for (Size1 j = 0; j < indices.size(); ++j)\n {\n int x_index = indices[j] % data->cloud_->width;\n int y_index = indices[j] / data->cloud_->width;\n int bin_x = x_index / local_model_->binSize().x();\n int bin_y = y_index / local_model_->binSize().y();\n error_vec[bin_x + delta_x * bin_y]->addIndex(x_index, y_index);\n }\n\n for (Size1 j = 0; j < error_vec.size(); ++j)\n {\n if (error_vec[j]->size() == 0)\n continue;\n error_vec[j]->createCloudWithIndices();\n\n int x_index = j % delta_x;\n int y_index = j / delta_x;\n\n// ceres::CostFunction * cost_function = new LocalCostFunction(error_vec[j], ceres::DO_NOT_TAKE_OWNERSHIP, error_vec[j]->size());\n ceres::CostFunction * cost_function = new LocalCostFunction(error_vec[j], ceres::DO_NOT_TAKE_OWNERSHIP, 3 * error_vec[j]->size());\n problem.AddResidualBlock(cost_function, NULL,\n local_model_->matrix()->at(x_index, y_index).data(),\n local_model_->matrix()->at(x_index, y_index + 1).data(),\n local_model_->matrix()->at(x_index + 1, y_index).data(),\n local_model_->matrix()->at(x_index + 1, y_index + 1).data());\n\n }\n\n }\n\n\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;\n options.max_num_iterations = 10;\n options.minimizer_progress_to_stdout = true;\n options.num_threads = 8;\n\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n\n for (Size1 i = 0; i < all_errors_vec.size(); ++i)\n delete all_errors_vec[i];\n\n}\n\nvoid DepthUndistortionEstimation::estimateGlobalModel()\n{\n#pragma omp parallel for\n for (size_t i = 0; i < data_vec_.size(); ++i)\n {\n DepthData & data = *data_vec_[i];\n const Checkerboard & gt_cb = *data.checkerboard_;\n const PCLCloud3 & cloud = *data.cloud_;\n\n Point3 und_color_cb_center = gt_cb.center();\n InverseGlobalMatrixEigen inverse_global(inverse_global_fit_->model());\n inverse_global.undistort(0, 0, und_color_cb_center);\n\n// RGBD_INFO(data.id_, \" - Transformed z: \" << gt_cb.center().z() << \" -> \" << und_color_cb_center.z());\n\n PCLCloud3::Ptr und_cloud = boost::make_shared(cloud);\n LocalMatrixPCL local(local_fit_->model());\n local.undistort(*und_cloud);\n\n PlaneInfo plane_info;\n\n if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))\n {\n data.estimated_plane_ = plane_info;\n data.undistorted_cloud_ = und_cloud;\n data.plane_extracted_ = true;\n\n#pragma omp critical\n {\n Indices reduced = *plane_info.indices_;\n std::random_shuffle(reduced.begin(), reduced.end());\n //reduced.resize(reduced.size() / 5);\n global_fit_->accumulateCloud(*und_cloud, reduced);\n global_fit_->addAccumulatedPoints(gt_cb.plane());\n }\n }\n else\n RGBD_WARN(data.id_, \"Plane not extracted!!\");\n\n }\n global_fit_->update();\n\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/offline_calibration_node.cpp", "content": "/*\n * offline_calibration_node.cpp\n *\n * Created on: Nov 28, 2012\n * Author: Filippo Basso\n */\n\n#include \n\n#include \n\n#include \n\n#include \n\n#include \n#include \n\n#include \n#include \n\n//#include \n#include \n\nusing namespace camera_info_manager;\nusing namespace calibration_msgs;\n\nnamespace fs = boost::filesystem;\n\nnamespace calibration\n{\n\nOfflineCalibrationNode::OfflineCalibrationNode (ros::NodeHandle & node_handle)\n : CalibrationNode(node_handle),\n\tstarting_index_(0)\n{\n if (not node_handle_.getParam(\"path\", path_))\n ROS_FATAL(\"Missing \\\"path\\\" parameter!!\");\n\n if (path_[path_.size() - 1] != '/')\n path_.append(\"/\");\n\n if (not node_handle_.getParam(\"instances\", instances_))\n ROS_FATAL(\"Missing \\\"instances\\\" parameter!!\");\n\n node_handle_.param(\"image_filename\", image_filename_, std::string(\"image_\"));\n node_handle_.param(\"cloud_filename\", cloud_filename_, std::string(\"cloud_\"));\n\n std::string depth_type_s;\n node_handle_.param(\"depth_type\", depth_type_s, std::string(\"none\"));\n if (depth_type_s == \"kinect1_depth\")\n depth_type_ = KINECT1_DEPTH;\n else if (depth_type_s == \"swiss_ranger_depth\")\n depth_type_ = SWISS_RANGER_DEPTH;\n else\n ROS_FATAL(\"Missing \\\"depth_type\\\" parameter!! Use \\\"kinect1_depth\\\" or \\\"swiss_ranger_depth\\\"\");\n}\n\nvoid\nOfflineCalibrationNode::spin ()\n{\n\n fs::path path(path_);\n fs::directory_iterator end_it;\n\n std::map cloud_file_map, image_file_map;\n\n for (fs::directory_iterator it(path); it != end_it; ++it)\n {\n fs::path file = it->path();\n if (fs::is_regular_file(file))\n {\n std::string file_name = file.filename().string();\n\n if (file_name.substr(0, cloud_filename_.size()) == cloud_filename_)\n {\n std::string id = file_name.substr(cloud_filename_.size(), file_name.find_last_of('.') - cloud_filename_.size());\n cloud_file_map[id] = file.string();\n }\n else if (file_name.substr(0, image_filename_.size()) == image_filename_)\n {\n std::string id = file_name.substr(image_filename_.size(), file_name.find_last_of('.') - image_filename_.size());\n image_file_map[id] = file.string();\n }\n }\n }\n\n typedef std::map::const_iterator map_iterator;\n\n int added = 0;\n\n ROS_INFO(\"Getting data...\");\n for (map_iterator cloud_it = cloud_file_map.begin(); cloud_it != cloud_file_map.end() and added < instances_; ++cloud_it)\n {\n map_iterator image_it = image_file_map.find(cloud_it->first);\n if (image_it != image_file_map.end())\n {\n cv::Mat image = cv::imread(image_it->second);\n\n if (not image.data)\n {\n ROS_WARN_STREAM(image_it->second << \" not valid!\");\n continue;\n }\n\n PCLCloud3::Ptr cloud;\n\n pcl::PCDReader pcd_reader;\n if (depth_type_ == KINECT1_DEPTH)\n {\n cloud = boost::make_shared();\n\n if (pcd_reader.read(cloud_it->second, *cloud) < 0)\n {\n ROS_WARN_STREAM(cloud_it->second << \" not valid!\");\n continue;\n }\n\n /*const Scalar & fx = depth_sensor_->cameraModel()->fx() * depth_sensor_->cameraModel()->binningX();\n const Scalar & fy = depth_sensor_->cameraModel()->fy() * depth_sensor_->cameraModel()->binningY();\n const Scalar & cx = depth_sensor_->cameraModel()->cx() * depth_sensor_->cameraModel()->binningX();\n const Scalar & cy = depth_sensor_->cameraModel()->cy() * depth_sensor_->cameraModel()->binningY();*/\n\n cv::Matx33d K = /*static_cast(depth_sensor_->cameraModel()->binningX()) **/ depth_sensor_->cameraModel()->fullIntrinsicMatrix();\n\n for (int j = 0; j < cloud->height; ++j)\n {\n for (int k = 0; k < cloud->width; ++k)\n {\n /*cloud->at(k, j).x = (k - cx) * cloud->at(k, j).z / fx;\n cloud->at(k, j).y = (j - cy) * cloud->at(k, j).z / fy;*/\n Scalar z = cloud->at(k, j).z;\n Point2 normalized_pixel((k - K(0, 2)) / K(0, 0), (j - K(1, 2)) / K(1, 1));\n\n Point3 p = z * depth_sensor_->cameraModel()->undistort2d_(normalized_pixel).homogeneous();\n\n cloud->at(k, j).x = p.x();\n cloud->at(k, j).y = p.y();\n }\n }\n\n /*for (size_t v = 0; v < cloud->height; ++v)\n {\n for (size_t u = 0; u < cloud->width; ++u)\n {\n PCLPoint3 & pt = cloud->points[u + v * cloud->width];\n// pt.x = (u - 314.5) * pt.z / 575.8157348632812;\n// pt.y = (v - 235.5) * pt.z / 575.8157348632812;\n// pt.x = (u - 309.7947658766498) * pt.z / 584.3333129882812;\n// pt.y = (v - 245.9642466885198) * pt.z / 582.8702392578125;\n pt.x = (u - depth_sensor_->cameraModel()->cx()) * pt.z / depth_sensor_->cameraModel()->fx();\n pt.y = (v - depth_sensor_->cameraModel()->cy()) * pt.z / depth_sensor_->cameraModel()->fy();\n }\n }*/\n\n// pcl::PCDWriter pcd_writer;\n// pcd_writer.write(cloud_it->second + \"_rev.pcd\", *cloud);\n\n }\n else if (depth_type_ == SWISS_RANGER_DEPTH)\n {\n// pcl::PCLPointCloud2Ptr pcl_cloud = boost::make_shared();\n// sensor_msgs::PointCloud2Ptr ros_cloud = boost::make_shared();\n// sr::Utility sr_utility;\n// if (pcd_reader.read(cloud_it->second, *pcl_cloud) < 0)\n// {\n// ROS_WARN_STREAM(cloud_it->second << \" not valid!\");\n// continue;\n// }\n// pcl_conversions::fromPCL(*pcl_cloud, *ros_cloud);\n//\n// sr_utility.setConfidenceThreshold(0.90f);\n// sr_utility.setInputCloud(ros_cloud);\n// sr_utility.setIntensityType(sr::Utility::INTENSITY_8BIT);\n// sr_utility.setConfidenceType(sr::Utility::CONFIDENCE_8BIT);\n// sr_utility.setNormalizeIntensity(true);\n// sr_utility.split(sr::Utility::CLOUD);\n//\n// cloud = sr_utility.cloud();\n }\n\n calibration_->addData(image, cloud);\n ++added;\n\n if (added == instances_)\n break;\n\n ROS_DEBUG_STREAM(\"(\" << image_it->second << \", \" << cloud_it->second << \") added.\");\n }\n }\n\n ROS_INFO_STREAM(added << \" images + point clouds added.\");\n\n geometry_msgs::Pose pose_msg;\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Initial transform:\\n\" << pose_msg);\n\n calibration_->perform();\n\n PolynomialUndistortionMatrixIO local_io;\n local_io.write(*calibration_->localModel(), path_ + \"local_matrix.txt\");\n\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Estimated transform:\\n\" << pose_msg);\n\n calibration_->optimize();\n\n PolynomialUndistortionMatrixIO global_io;\n global_io.write(*calibration_->globalModel(), path_ + \"global_matrix.txt\");\n\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Optimized transform:\\n\" << pose_msg);\n\n std::ofstream transform_file;\n transform_file.open((path_ + \"camera_pose.yaml\").c_str());\n transform_file << pose_msg;\n transform_file.close();\n\n const std::vector & depth_intrinsics = calibration_->optimizedIntrinsics();\n\n ROS_INFO_STREAM(\"fx = \" << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[0]);\n ROS_INFO_STREAM(\"fy = \" << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[1]);\n ROS_INFO_STREAM(\"cx = \" << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[2]);\n ROS_INFO_STREAM(\"cy = \" << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[3]);\n\n std::ofstream intrinsics_file;\n intrinsics_file.open((path_ + \"depth_intrinsics.yaml\").c_str());\n intrinsics_file << \"intrinsics:\" << std::endl;\n intrinsics_file << \" fx: \" << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[0] << std::endl;\n intrinsics_file << \" fy: \" << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[1] << std::endl;\n intrinsics_file << \" cx: \" << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[2] << std::endl;\n intrinsics_file << \" cy: \" << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[3] << std::endl;\n intrinsics_file.close();\n\n\n// ros::Rate rate(1.0);\n\n// while (ros::ok())\n// {\n// calibration_->publishData();\n// rate.sleep();\n// }\n\n}\n\n} /* namespace calibration */\n\nint\nmain (int argc,\n char ** argv)\n{\n ros::init(argc, argv, \"offline_calibration\");\n ros::NodeHandle node_handle(\"~\");\n\n try\n {\n calibration::OfflineCalibrationNode calib_node(node_handle);\n if (not calib_node.initialize())\n return 0;\n calib_node.spin();\n }\n catch (const std::runtime_error & error)\n {\n ROS_FATAL_STREAM(\"Calibration error: \" << error.what());\n return 1;\n }\n\n return 0;\n}\n\n" }, { "path": "src/rgbd_calibration/publisher.cpp", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#include \n#include \n#include \n#include \n\n#include \n\nnamespace calibration\n{\n\nPublisher::Publisher(const ros::NodeHandle & node_handle)\n : node_handle_(node_handle)\n{\n marker_pub_ = node_handle_.advertise(\"rgbd_markers\", 0);\n}\n\nvoid Publisher::publishTF(const BaseObject & object)\n{\n geometry_msgs::TransformStamped transform_msg;\n object.toTF(transform_msg);\n tf_pub_.sendTransform(transform_msg);\n}\n\n//void Publisher::publish(const RGBDCheckerboard & rgbd_checkerboard,\n// const DistortionMap & map)\n//{\n// publish(rgbd_checkerboard);\n//\n// visualization_msgs::Marker cloud_marker;\n// cloud_marker.ns = \"undist_cloud\";\n//\n// Point3Matrix points = rgbd_checkerboard.depthView()->interestPoints();\n// map.undistort(points);\n//\n// cloud_marker.header.stamp = ros::Time::now();\n// cloud_marker.header.frame_id = rgbd_checkerboard.depthView()->sensor()->frameId();\n// points.toMarker(cloud_marker);\n// cloud_marker.color.r = 0.0;\n// cloud_marker.color.g = 1.0;\n// cloud_marker.color.b = 0.0;\n//\n// CheckerboardPublisherSet & pub_set = c_pub_map_[rgbd_checkerboard.id()];\n// pub_set.marker_pub_.publish(cloud_marker);\n//}\n\nvoid Publisher::publish(const CheckerboardViews & rgbd_checkerboard,\n const std::string & prefix)\n{\n std::stringstream ss;\n ss << prefix << \"checkerboard_\" << rgbd_checkerboard.id();\n std::string ns = ss.str();\n\n if (c_pub_map_.find(ns) == c_pub_map_.end())\n {\n CheckerboardPublisherSet pub_set;\n\n std::stringstream sss;\n sss << ns << \"/markers\";\n pub_set.marker_pub_ = node_handle_.advertise(sss.str(), 0);\n\n c_pub_map_[ns] = pub_set;\n }\n\n visualization_msgs::Marker checkerboard_marker;\n visualization_msgs::Marker cloud_marker;\n visualization_msgs::Marker d_plane_marker;\n\n checkerboard_marker.ns = \"checkerboard\";\n cloud_marker.ns = \"cloud\";\n d_plane_marker.ns = \"d_plane\";\n\n checkerboard_marker.id = cloud_marker.id = d_plane_marker.id = 0;\n\n CheckerboardPublisherSet & pub_set = c_pub_map_[ns];\n\n if (rgbd_checkerboard.colorCheckerboard())\n {\n rgbd_checkerboard.colorCheckerboard()->toMarker(checkerboard_marker);\n pub_set.marker_pub_.publish(checkerboard_marker);\n }\n\n// if (rgbd_checkerboard.depthPlane())\n// {\n// rgbd_checkerboard.depthPlane()->toMarker(d_plane_marker);\n// pub_set.marker_pub_.publish(d_plane_marker);\n// }\n\n// if (rgbd_checkerboard.depthView())\n// {\n// rgbd_checkerboard.depthView()->toMarker(cloud_marker);\n// pub_set.marker_pub_.publish(cloud_marker);\n// }\n}\n\nvoid Publisher::publish(const RGBDData & rgbd)\n{\n std::stringstream ss;\n ss << \"rgbd_\" << rgbd.id();\n std::string ns = ss.str();\n\n if (d_pub_map_.find(rgbd.id()) == d_pub_map_.end())\n {\n DataPublisherSet pub_set;\n\n ss.str(\"\");\n ss << ns << \"/cloud\";\n pub_set.cloud_pub_ = node_handle_.advertise(ss.str(), 0);\n\n // ss.str(\"\");\n // ss << ns << \"/image\";\n // pub_set.image_pub_ = node_handle_.advertise(ss.str(), 0);\n\n ss.str(\"\");\n ss << ns << \"/rgbd\";\n pub_set.rgbd_pub_ = node_handle_.advertise(ss.str(), 0);\n\n d_pub_map_[rgbd.id()] = pub_set;\n }\n\n DataPublisherSet & pub_set = d_pub_map_[rgbd.id()];\n\n// rgbd.depthData()->header.stamp = ros::Time::now().toNSec();\n pub_set.cloud_pub_.publish(*rgbd.depthData());\n rgbd.fuseData();\n pub_set.rgbd_pub_.publish(*rgbd.fusedData());\n\n}\n\n} /* namespace calibration */\n" }, { "path": "src/rgbd_calibration/simulation_node.cpp", "content": "/*\n * Copyright (C) 2013 - Filippo Basso \n *\n * This program is free software: you can redistribute it and/or modify\n * it under the terms of the GNU General Public License as published by\n * the Free Software Foundation, either version 3 of the License, or\n * (at your option) any later version.\n *\n * This program is distributed in the hope that it will be useful,\n * but WITHOUT ANY WARRANTY; without even the implied warranty of\n * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n * GNU General Public License for more details.\n *\n * You should have received a copy of the GNU General Public License\n * along with this program. If not, see .\n */\n\n#include \n#include \n\n#include \n#include \n\n#include \n\n#include \n\nnamespace calibration\n{\n\nSimulationNode::SimulationNode(ros::NodeHandle & node_handle)\n : CalibrationNode(node_handle)\n{\n\n // if (not node_handle_.getParam(\"instances\", instances_))\n // ROS_FATAL(\"Missing \\\"instances\\\" parameter!!\");\n\n node_handle_.param(\"depth_error\", depth_error_, 0.0035);\n node_handle_.param(\"image_error\", image_error_, 0.5);\n\n node_handle_.param(\"min_distance\", min_distance_, 1.0);\n node_handle_.param(\"max_distance\", max_distance_, 4.0);\n node_handle_.param(\"step\", step_, 1.0);\n\n}\n\nvoid SimulationNode::spin()\n{\n ros::Rate rate(10.0);\n\n int added = 0;\n\n Scalar aa = -0.01, bb = 0.96, cc = 0.0;\n GlobalPolynomial::Coefficients global_error_function_coeffs(cc, bb, aa);\n GlobalPolynomial global_error_function(global_error_function_coeffs);\n\n ROS_INFO_STREAM(global_error_function_coeffs.transpose());\n\n ROS_INFO(\"Getting data...\");\n for (double z = min_distance_; z <= max_distance_ + 0.01; z += step_)\n {\n Translation3 translation_cb_center(-cb_vec_[0]->center());\n Translation3 translation_z(0, 0, z);\n\n boost::mt19937 random_gen(time(0));\n\n boost::normal_distribution<> depth_error(0.0, depth_error_);\n boost::variate_generator > depth_noise(random_gen, depth_error);\n\n boost::normal_distribution<> image_error(0.0, image_error_);\n boost::variate_generator > image_noise(random_gen, image_error);\n\n#pragma omp parallel for\n for (int i = 0; i < 9; ++i)\n {\n Transform T = Transform::Identity();\n\n if (i % 3 != 1)\n {\n AngleAxis r;\n r.axis() = Vector3::UnitY();\n r.angle() = (i % 3 == 2 ? M_PI / 6 : -M_PI / 6);\n T.prerotate(r);\n }\n if (i / 3 != 1)\n {\n AngleAxis r;\n r.axis() = Vector3::UnitX();\n r.angle() = (i / 3 == 0 ? M_PI / 6 : -M_PI / 6);\n T.prerotate(r);\n }\n\n T = translation_z * T * translation_cb_center;\n\n Checkerboard::Ptr real_cb = boost::make_shared(*cb_vec_[0]);\n real_cb->transform(T);\n\n // Create image\n\n cv::Mat image(480, 640, CV_8UC3, cv::Scalar(255, 255, 255));\n Cloud2 image_corners = color_sensor_->cameraModel()->project3dToPixel(real_cb->corners());\n\n std::vector corners;\n for (Size1 c = 0; c < image_corners.elements(); ++c)\n {\n if (image_corners[c].x() > 0 and image_corners[c].x() < image.cols and image_corners[c].y() > 0\n and image_corners[c].y() < image.rows)\n corners.push_back(cv::Point2f(image_corners[c].x(), image_corners[c].y()));\n\n image_corners[c].x() += image_noise();\n image_corners[c].y() += image_noise();\n\n }\n cv::drawChessboardCorners(image, cv::Size(image_corners.size().x(), image_corners.size().y()), corners,\n corners.size() == image_corners.elements());\n\n // cv::imshow(\"AAA\", image);\n // cv::waitKey(100);\n\n // Create point cloud\n\n PCLCloud3::Ptr cloud = boost::make_shared();\n std::vector inliers;\n\n real_cb->transform(color_sensor_->pose());\n\n for (int y = 0; y < 480; ++y)\n {\n for (int x = 0; x < 640; ++x)\n {\n Line line(Point3::Zero(),\n color_sensor_->cameraModel()->projectPixelTo3dRay(Point2(x, y)));\n Point3 point = line.intersectionPoint(real_cb->plane());\n Scalar new_z = (-bb + std::sqrt(bb * bb - 4 * aa * (cc - point.z()))) / (2 * aa);\n new_z = new_z + depth_noise() * new_z * new_z;\n point *= new_z / point.z();\n PCLPoint3 pcl_point;\n pcl_point.x = point.x();\n pcl_point.y = point.y();\n pcl_point.z = point.z();\n cloud->points.push_back(pcl_point);\n inliers.push_back(inliers.size());\n }\n }\n\n cloud->width = 640;\n cloud->height = 480;\n\n // pcl::visualization::PCLVisualizer vis;\n // vis.addPointCloud(cloud);\n // vis.spin();\n\n RGBDData::Ptr data = boost::make_shared(i + z * 3);\n data->setColorSensor(color_sensor_);\n data->setDepthSensor(depth_sensor_);\n data->setColorData(image);\n data->setDepthData(*cloud);\n\n std::stringstream ss;\n ss << \"view_\" << data->id() << \"_\" << i;\n\n CheckerboardViews::Ptr cb_views = boost::make_shared(ss.str());\n cb_views->setData(data);\n cb_views->setCheckerboard(cb_vec_[0]);\n cb_views->setImageCorners(image_corners);\n //cb_views->setPlaneInliers(inliers, 0.01);\n\n#pragma omp critical\n {\n calibration_->addCheckerboardViews(cb_views);\n ++added;\n }\n }\n\n //rate.sleep();\n }\n\n ROS_INFO_STREAM(\"Added \" << added << \" checkerboards.\");\n\n geometry_msgs::Pose pose_msg;\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Initial transform:\\n\" << pose_msg);\n\n calibration_->setEstimateDepthUndistortionModel(true);\n //calibration_->setEstimateDepthUndistortionModel(false);\n calibration_->perform();\n\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Estimated transform:\\n\" << pose_msg);\n\n calibration_->optimize();\n\n tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);\n ROS_INFO_STREAM(\"Optimized transform:\\n\" << pose_msg);\n\n rate = ros::Rate(1.0);\n\n while (ros::ok())\n {\n calibration_->publishData();\n rate.sleep();\n }\n\n}\n\n} /* namespace calibration */\n\nint main(int argc,\n char ** argv)\n{\n ros::init(argc, argv, \"simulation\");\n ros::NodeHandle node_handle(\"~\");\n\n try\n {\n calibration::SimulationNode sim_node(node_handle);\n if (not sim_node.initialize())\n return 0;\n sim_node.spin();\n\n ros::spin();\n }\n catch (std::runtime_error & error)\n {\n ROS_FATAL(\"Calibration error: %s\", error.what());\n return 1;\n }\n\n return 0;\n}\n\n" }, { "path": "src/rgbd_calibration/test_node.cpp", "content": "#include \n\n#include \n\n#include \n\n#include \n#include \n\n#include \n\n#include \n\n//#include \n\nusing namespace camera_info_manager;\nusing namespace calibration_msgs;\n\nnamespace calibration\n{\n\nTestNode::TestNode(ros::NodeHandle & node_handle)\n : node_handle_(node_handle)\n{\n checkerboards_sub_ = node_handle_.subscribe(\"checkerboard_array\", 1, &TestNode::checkerboardArrayCallback, this);\n\n if (not node_handle_.getParam(\"camera_calib_url\", camera_calib_url_))\n ROS_FATAL(\"Missing \\\"camera_calib_url\\\" parameter!!\");\n node_handle_.param(\"camera_name\", camera_name_, std::string(\"camera\"));\n\n if (not node_handle_.getParam(\"depth_camera_calib_url\", depth_camera_calib_url_))\n ROS_FATAL(\"Missing \\\"depth_camera_calib_url\\\" parameter!!\");\n node_handle_.param(\"depth_camera_name\", depth_camera_name_, std::string(\"depth_camera\"));\n\n node_handle_.param(\"downsample_ratio\", downsample_ratio_, 1);\n if (downsample_ratio_ < 1)\n {\n downsample_ratio_ = 1;\n ROS_WARN(\"\\\"downsample_ratio\\\" cannot be < 1. Skipping.\");\n }\n\n#if (!defined(UNCALIBRATED)) || (defined(UNCALIBRATED) && defined(SKEW))\n if (node_handle_.hasParam(\"camera_pose\"))\n {\n Translation3 translation;\n Quaternion rotation;\n\n node_handle_.getParam(\"camera_pose/position/x\", translation.vector().coeffRef(0));\n node_handle_.getParam(\"camera_pose/position/y\", translation.vector().coeffRef(1));\n node_handle_.getParam(\"camera_pose/position/z\", translation.vector().coeffRef(2));\n\n node_handle_.getParam(\"camera_pose/orientation/x\", rotation.coeffs().coeffRef(0));\n node_handle_.getParam(\"camera_pose/orientation/y\", rotation.coeffs().coeffRef(1));\n node_handle_.getParam(\"camera_pose/orientation/z\", rotation.coeffs().coeffRef(2));\n node_handle_.getParam(\"camera_pose/orientation/w\", rotation.coeffs().coeffRef(3));\n\n rotation.normalize();\n\n camera_pose_ = translation * rotation;\n }\n else\n ROS_FATAL(\"Missing \\\"camera_pose\\\" parameter!!\");\n#else\n camera_pose_ = Eigen::Affine3d::Identity() * Translation3(0.052, 0.0, 0.0);\n#endif\n\n\n\n if (node_handle_.hasParam(\"local_und_matrix_file\"))\n node_handle_.getParam(\"local_und_matrix_file\", local_matrix_file_);\n else\n ROS_FATAL(\"Missing \\\"local_und_matrix_file\\\" parameter!!\");\n\n if (node_handle_.hasParam(\"global_und_matrix_file\"))\n node_handle_.getParam(\"global_und_matrix_file\", global_matrix_file_);\n else\n ROS_FATAL(\"Missing \\\"global_und_matrix_file\\\" parameter!!\");\n\n if (not node_handle_.getParam(\"path\", path_))\n ROS_FATAL(\"Missing \\\"path\\\" parameter!!\");\n\n if (path_[path_.size() - 1] != '/')\n path_.append(\"/\");\n\n bool ok = true;\n if (not node_handle_.hasParam(\"intrinsics\"))\n ROS_FATAL(\"Missing \\\"intrinsics\\\" parameter!!\");\n depth_intrinsics_.resize(4);\n ok = ok and node_handle_.getParam(\"intrinsics/fx\", depth_intrinsics_[0]);\n ok = ok and node_handle_.getParam(\"intrinsics/fy\", depth_intrinsics_[1]);\n ok = ok and node_handle_.getParam(\"intrinsics/cx\", depth_intrinsics_[2]);\n ok = ok and node_handle_.getParam(\"intrinsics/cy\", depth_intrinsics_[3]);\n#ifdef SKEW\n ok = ok and node_handle_.getParam(\"intrinsics/s\", depth_intrinsics_[4]);\n#endif\n if (not ok)\n ROS_FATAL(\"Malformed \\\"intrinsics\\\" parameter!!\");\n\n if (not node_handle_.getParam(\"instances\", instances_))\n ROS_FATAL(\"Missing \\\"instances\\\" parameter!!\");\n\n node_handle_.param(\"image_extension\", image_extension_, std::string(\"png\"));\n node_handle_.param(\"starting_index\", starting_index_, 1);\n node_handle_.param(\"image_filename\", image_filename_, std::string(\"image_\"));\n node_handle_.param(\"cloud_filename\", cloud_filename_, std::string(\"cloud_\"));\n\n node_handle_.param(\"only_show\", only_show_, false);\n\n int images_size_x, images_size_y;\n node_handle_.param(\"depth_image/cols\", images_size_x, 640);\n node_handle_.param(\"depth_image/rows\", images_size_y, 480);\n images_size_.x() = images_size_x / downsample_ratio_;\n images_size_.y() = images_size_y / downsample_ratio_;\n\n std::string depth_type_s;\n node_handle_.param(\"depth_type\", depth_type_s, std::string(\"none\"));\n if (depth_type_s == \"kinect1_depth\")\n depth_type_ = KINECT1_DEPTH;\n else if (depth_type_s == \"swiss_ranger_depth\")\n depth_type_ = SWISS_RANGER_DEPTH;\n else\n ROS_FATAL(\"Missing \\\"depth_type\\\" parameter!! Use \\\"kinect1_depth\\\" or \\\"swiss_ranger_depth\\\"\");\n\n}\n\nbool TestNode::initialize()\n{\n if (not waitForMessages())\n return false;\n\n for (size_t i = 0; i < checkerboard_array_msg_->checkerboards.size(); ++i)\n cb_vec_.push_back(createCheckerboard(checkerboard_array_msg_->checkerboards[i], i));\n\n BaseObject::Ptr world = boost::make_shared();\n world->setFrameId(\"/world\");\n\n CameraInfoManager depth_manager(node_handle_, depth_camera_name_, depth_camera_calib_url_);\n sensor_msgs::CameraInfo msg = depth_manager.getCameraInfo();\n /*msg.P[0] = depth_intrinsics_[0];\n msg.P[5] = depth_intrinsics_[1];\n msg.P[2] = depth_intrinsics_[2];\n msg.P[6] = depth_intrinsics_[3];\n msg.K[0] = depth_intrinsics_[0];\n msg.K[4] = depth_intrinsics_[1];\n msg.K[2] = depth_intrinsics_[2];\n msg.K[5] = depth_intrinsics_[3];\n\n#ifdef SKEW\n msg.P[1] = depth_intrinsics_[4];\n msg.K[1] = depth_intrinsics_[4];\n#endif*/\n\n KinectDepthCameraModel::ConstPtr depth_pinhole_model = boost::make_shared(msg);\n\n depth_sensor_ = boost::make_shared >();\n depth_sensor_->setFrameId(\"/depth_sensor\");\n depth_sensor_->setParent(world);\n depth_sensor_->setCameraModel(depth_pinhole_model);\n// Polynomial depth_error_function(KINECT_ERROR_POLY); // TODO add parameter\n// depth_sensor_->setDepthErrorFunction(depth_error_function);\n\n CameraInfoManager manager(node_handle_, camera_name_, camera_calib_url_);\n msg = manager.getCameraInfo();\n\n PinholeCameraModel::ConstPtr pinhole_model = boost::make_shared(manager.getCameraInfo());\n cv::Mat P = cv::getOptimalNewCameraMatrix(depth_pinhole_model->fullIntrinsicMatrix(), depth_pinhole_model->distortionCoeffs(),\n depth_pinhole_model->fullResolution(), 0);\n\n std::cout << P << std::endl;\n\n color_sensor_ = boost::make_shared();\n color_sensor_->setFrameId(\"/pinhole_sensor\");\n color_sensor_->setCameraModel(pinhole_model);\n color_sensor_->setParent(depth_sensor_);\n color_sensor_->transform(camera_pose_);\n\n LocalModel::Data::Ptr local_und_data;\n\n PolynomialUndistortionMatrixIO local_io;\n if (not local_io.read(local_und_data, local_matrix_file_))\n ROS_FATAL_STREAM(\"File \" << local_matrix_file_ << \" not found!!\");\n\n GlobalModel::Data::Ptr global_data;\n\n PolynomialUndistortionMatrixIO global_io;\n if (not global_io.read(global_data, global_matrix_file_))\n ROS_FATAL_STREAM(\"File \" << global_matrix_file_ << \" not found!!\");\n\n LocalModel::Ptr local_model = boost::make_shared(images_size_);\n local_model->setMatrix(local_und_data);\n\n LocalMatrixPCL::Ptr local_und = boost::make_shared();\n local_und->setModel(local_model);\n\n GlobalModel::Ptr global_model = boost::make_shared(images_size_);\n global_model->setMatrix(global_data);\n\n GlobalMatrixPCL::Ptr global_matrix = boost::make_shared();\n global_matrix->setModel(global_model);\n\n UndistortionModel::Ptr model = boost::make_shared();\n model->setLocalModel(local_model);\n model->setGlobalModel(global_model);\n\n// UndistortionPCL::Ptr undistortion = boost::make_shared();\n// undistortion->setModel(model);\n\n depth_sensor_->setUndistortionModel(model);\n\n publisher_ = boost::make_shared(node_handle_);\n\n test_ = boost::make_shared();\n\n test_->setCheckerboards(cb_vec_);\n test_->setDepthSensor(depth_sensor_);\n test_->setColorSensor(color_sensor_);\n test_->setLocalModel(local_model);\n test_->setGlobalModel(global_model);\n test_->setPublisher(publisher_);\n test_->setDownSampleRatio(downsample_ratio_);\n\n return true;\n}\n\nvoid TestNode::checkerboardArrayCallback(const CheckerboardArray::ConstPtr & msg)\n{\n checkerboard_array_msg_ = msg;\n}\n\nbool TestNode::waitForMessages() const\n{\n ros::Rate rate(1.0);\n ros::spinOnce();\n while (ros::ok() and (not checkerboard_array_msg_))\n {\n ROS_WARN(\"Not all messages received!\");\n rate.sleep();\n ros::spinOnce();\n }\n return checkerboard_array_msg_;\n}\n\nCheckerboard::Ptr TestNode::createCheckerboard(const CheckerboardMsg::ConstPtr & msg,\n int id)\n{\n return createCheckerboard(*msg, id);\n}\n\nCheckerboard::Ptr TestNode::createCheckerboard(const CheckerboardMsg & msg,\n int id)\n{\n std::stringstream ss;\n ss << \"/checkerboard_\" << id;\n Checkerboard::Ptr cb = boost::make_shared(msg.cols, msg.rows, msg.cell_width, msg.cell_height);\n cb->setFrameId(ss.str());\n return cb;\n}\n\nvoid TestNode::spin()\n{\n ros::Rate rate(10.0);\n\n int added = 0;\n\n /*std::fstream fs;\n fs.open(\"/tmp/points.txt\", std::fstream::out);\n fs.close();*/\n\n ROS_INFO(\"Getting data...\");\n for (int i = starting_index_; ros::ok() and i < starting_index_ + instances_; ++i)\n {\n\n std::stringstream image_file, cloud_file, depth_file;\n image_file << path_ << image_filename_ << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".\" << image_extension_;\n cloud_file << path_ << cloud_filename_ << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".pcd\";\n depth_file << path_ << \"depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n\n cv::Mat image = cv::imread(image_file.str());\n\n if (not image.data)\n continue;\n\n PCLCloud3::Ptr cloud;\n\n pcl::PCDReader pcd_reader;\n if (depth_type_ == KINECT1_DEPTH)\n {\n cloud = boost::make_shared();\n\n if (pcd_reader.read(cloud_file.str(), *cloud) < 0)\n {\n ROS_WARN_STREAM(cloud_file.str() << \" not valid!\");\n continue;\n }\n\n /*cv::Mat depth = cv::imread(depth_file.str(), 2);\n cloud->width = depth.cols;\n cloud->height = depth.rows;\n cloud->points.resize(cloud->width * cloud->height);\n cloud->is_dense = false;\n\n for (int k = 0; k < cloud->width; ++k)\n {\n for (int j = 0; j < cloud->height; ++j)\n {\n if (depth.at(j, k) > 0)\n cloud->at(k, j).z = depth.at(j, k) / 1000.0f;\n else\n cloud->at(k, j).z = std::numeric_limits::quiet_NaN();\n }\n }*/\n\n }\n\n#ifdef HERRERA\n PCLCloud3::Ptr cloud_ = PCLCloud3::Ptr(new PCLCloud3(cloud->width, cloud->height));\n cloud_->header = cloud->header;\n cloud_->is_dense = cloud->is_dense;\n for (size_t p_i = 0; p_i < cloud->points.size(); ++p_i)\n {\n int row = p_i % cloud->height;\n int column = p_i / cloud->height;\n int index = row * cloud->width + column;\n cloud_->points[index].x = cloud->points[p_i].x;\n cloud_->points[index].y = cloud->points[p_i].y;\n cloud_->points[index].z = cloud->points[p_i].z;\n\n }\n cloud = cloud_;\n#else\n# if (!defined(UNCALIBRATED)) || (defined(UNCALIBRATED) && defined(SKEW))\n //std::cout << cv::getOptimalNewCameraMatrix(color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(),\n //\t\tcolor_sensor_->cameraModel()->fullResolution(), 0) << std::endl;\n\n const Scalar & fx = depth_intrinsics_[0];\n const Scalar & fy = depth_intrinsics_[1];\n const Scalar & cx = depth_intrinsics_[2];\n const Scalar & cy = depth_intrinsics_[3];\n# ifdef SKEW\n const Scalar & s = depth_intrinsics_[4];\n# endif\n \n for (int k = 0; k < cloud->width; ++k)\n {\n for (int j = 0; j < cloud->height; ++j)\n {\n# ifdef SKEW\n // x = (z*(k*fy - cx*fy - j*s + cy*s))/(fx*fy)\n cloud->at(k, j).x = (k*fy - cx*fy - j*s + cy*s) * cloud->at(k, j).z / (fx*fy);\n# else\n cloud->at(k, j).x = (k - cx) * cloud->at(k, j).z / fx;\n# endif\n cloud->at(k, j).y = (j - cy) * cloud->at(k, j).z / fy;\n }\n }\n# endif\n#endif\n\n\n PCLCloudRGB::Ptr cloud_rgb = test_->addData(image, cloud);\n ++added;\n\n std::stringstream cloud_rgb_file;\n cloud_rgb_file << path_ << cloud_filename_ << (i < 10 ? \"000\" : \"00\") << i << \"_rgb.pcd\";\n\n pcl::PCDWriter pcd_writer;\n if (depth_type_ == KINECT1_DEPTH)\n {\n if (pcd_writer.write(cloud_rgb_file.str(), *cloud_rgb) < 0)\n {\n ROS_WARN_STREAM(cloud_rgb_file.str() << \" not valid!\");\n continue;\n }\n }\n\n //rate.sleep();\n }\n ROS_INFO_STREAM(\"Added \" << added << \" images + point clouds.\");\n if (not only_show_)\n {\n// test_->visualizeClouds();\n// test_->testPlanarityError();\n// test_->testCheckerboardError();\n test_->testCube();\n }\n\n\n rate = ros::Rate(1.0);\n\n while (ros::ok())\n {\n test_->publishData();\n rate.sleep();\n }\n\n}\n\nvoid TestNode::spin2()\n{\n ros::Rate rate(10.0);\n\n ROS_INFO(\"Getting data...\");\n for (int i = 1000; ros::ok() and i < 5000; ++i)\n {\n std::stringstream depth_file, depth_file_und;\n depth_file << path_ << \"depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n depth_file_und << path_ << \"und/depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n\n cv::Mat depth = cv::imread(depth_file.str(), 2);\n\n if (not depth.data)\n continue;\n\n for (int k = 0; k < depth.rows; ++k)\n {\n for (Size1 j = 0; j < depth.cols; ++j)\n {\n Scalar z = depth.at(k, j) / Scalar(1000.0);\n if (z == 0)\n continue;\n\n depth_sensor_->undistortionModel()->localModel()->undistort(j, k, z);\n depth_sensor_->undistortionModel()->globalModel()->undistort(j, k, z);\n\n depth.at(k, j) = cv::saturate_cast(cvRound(z * 1000));\n }\n }\n\n cv::imwrite(depth_file_und.str(), depth);\n\n }\n ROS_INFO(\"OK\");\n}\n\nvoid TestNode::spin3 ()\n{\n ros::Rate rate(10.0);\n\n const Scalar & fx = depth_intrinsics_[0];\n const Scalar & fy = depth_intrinsics_[1];\n const Scalar & cx = depth_intrinsics_[2];\n const Scalar & cy = depth_intrinsics_[3];\n\n Transform t_original = Eigen::Affine3d::Identity() * Translation3(-0.025, 0.0, 0.0);\n Transform t = color_sensor_->pose().inverse();\n\n ROS_INFO(\"Getting data...\");\n for (int i = 0; ros::ok() and i < 5000; ++i)\n {\n std::stringstream depth_file_2, depth_file, depth_file_und;\n depth_file << path_ << \"depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n depth_file_2 << path_ << \"alberto/depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n depth_file_und << path_ << \"alberto/und/depth_\" << (i < 10 ? \"000\" : (i < 100 ? \"00\" : (i < 1000 ? \"0\" : \"\"))) << i << \".png\";\n\n cv::Mat depth = cv::imread(depth_file.str(), 2);\n\n if (not depth.data)\n continue;\n\n cv::Mat_ depth_2 = cv::Mat(depth.size(), 0);\n cv::Mat_ depth_und = cv::Mat(depth.size(), 0);\n\n for (int k = 0; k < depth.rows; ++k)\n {\n for (int j = 0; j < depth.cols; ++j)\n {\n Scalar z = depth.at(k, j) / Scalar(1000.0);\n\n if (z > 0)\n {\n\n Point3 point_eigen;\n Point3 point_eigen_original;\n\n point_eigen.x() = (j - cx) * z / fx;\n point_eigen.y() = (k - cy) * z / fy;\n point_eigen.z() = z;\n\n point_eigen_original = t_original * point_eigen;\n Point2 point_image_original = color_sensor_->cameraModel()->project3dToPixel(point_eigen_original);\n\n uint16_t new_z_original = cv::saturate_cast(cvRound(point_eigen_original.z() * 1000));\n int x = cvRound(point_image_original[0]);\n int y = cvRound(point_image_original[1]);\n\n if ((x >= 0 and x < depth.cols and y >= 0 and y < depth.rows) and\n (depth_2.at(y, x) == 0 or new_z_original < depth_2.at(y, x)))\n depth_2.at(y, x) = new_z_original;\n\n\n depth_sensor_->undistortionModel()->localModel()->undistort(j, k, z);\n depth_sensor_->undistortionModel()->globalModel()->undistort(j, k, z);\n point_eigen.x() = (j - cx) * z / fx;\n point_eigen.y() = (k - cy) * z / fy;\n point_eigen.z() = z;\n\n point_eigen = t * point_eigen;\n Point2 point_image = color_sensor_->cameraModel()->project3dToPixel(point_eigen);\n\n uint16_t new_z = cv::saturate_cast(cvRound(point_eigen.z() * 1000));\n x = cvRound(point_image[0]);\n y = cvRound(point_image[1]);\n\n if ((x >= 0 and x < depth.cols and y >= 0 and y < depth.rows) and\n (depth_und.at(y, x) == 0 or new_z < depth_und.at(y, x)))\n depth_und.at(y, x) = new_z;\n\n }\n }\n }\n\n cv::imwrite(depth_file_und.str(), depth_und);\n cv::imwrite(depth_file_2.str(), depth_2);\n\n }\n ROS_INFO(\"OK\");\n}\n\n\n} /* namespace calibration */\n\nint main(int argc,\n char ** argv)\n{\n ros::init(argc, argv, \"test_calibration\");\n ros::NodeHandle node_handle(\"~\");\n\n try\n {\n calibration::TestNode test_node(node_handle);\n if (not test_node.initialize())\n return 0;\n test_node.spin();\n\n ros::spin();\n }\n catch (std::runtime_error & error)\n {\n ROS_FATAL(\"Test node error: %s\", error.what());\n return 1;\n }\n\n return 0;\n}\n" }, { "path": "test/polynomial_undistortion_matrix_multifit_test.cpp", "content": "#include \n#include \n#include \n#include \n\n#include \n#include \n\n//using namespace calibration;\n\n//typedef PolynomialUndistortionMatrixEigen > UMap;\n//typedef PolynomialUndistortionMatrixFitEigen > UMapFit;\n\n//TEST(PolynomialUndistortionMatrixFit, PolynomialUndistortionMatrixFit)\n//{\n// UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);\n// SUCCEED();\n//}\n//\n//TEST(PolynomialUndistortionMatrixFit, getIndex)\n//{\n// UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);\n// size_t x, y;\n// u_map.getIndex(UMap::toSphericalCoordinates(Types::Point3(0.01, -0.01, 1.0)), x, y);\n// EXPECT_EQ(5, x);\n// EXPECT_EQ(5, y);\n//}\n//\n//TEST(PolynomialUndistortionMatrixFit, updateDistortionMap)\n//{\n// UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);\n//\n// // applied distortion: z_d = c3 * z_p*z_p + c2 * z_p + c1\n// Types::Scalar c0 = 1.0;\n// Types::Scalar c1 = -2.0;\n// Types::Scalar c2 = 1.0;\n//\n// // gaussian noise on the detected point, linear with respect to the z coordinate.\n// boost::mt19937 rnd_gen;\n// boost::normal_distribution norm_dist(0.0, 1e-4);\n// boost::variate_generator > normal_gen(rnd_gen, norm_dist);\n//\n// // update ignore bins with less than 2*Dimension (6) points.\n// for (Types::Scalar z = 1.0; z <= 10.0; z++)\n// for (int n = 0; n < 10; n++)\n// u_map.addPoint(Types::Point3(0.0, 0.0, z), Types::Plane(-Eigen::Vector3d::UnitZ(), c2 * z * z + (c1 + normal_gen()) * z + c0));\n// u_map.update();\n//\n// size_t x, y;\n// u_map.getIndex(UMap::toSphericalCoordinates(Types::Point3(0.0, 0.0, 1.0)), x, y);\n// EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[0], c0, 1e-2);\n// EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[1], c1, 1e-2);\n// EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[2], c2, 1e-2);\n//}\n//\n//TEST(PolynomialUndistortionMatrixFit, accumulatePoint)\n//{\n// Types::Scalar c0 = 1.0;\n// Types::Scalar c1 = -2.0;\n// Types::Scalar c2 = 1.0;\n//\n// boost::mt19937 rnd_gen;\n// boost::normal_distribution norm_dist(0.0, 1e-4);\n// boost::variate_generator > normal_gen(rnd_gen, norm_dist);\n//\n// UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);\n// UMapFit u_map2(10, 10, M_PI / 3.0, M_PI / 5.0);\n//\n// for (double z = 1.0; z <= 10.0; z++)\n// {\n// for (int n = 0; n < 10; n++)\n// {\n// Types::Plane plane(-Eigen::Vector3d::UnitZ(), c2 * z * z + (c1 + normal_gen()) * z + c0);\n// u_map.addPoint(pcl::PointXYZ(0.0, 0.0, z), plane);\n// u_map2.accumulatePoint(pcl::PointXYZ(0.0, 0.0, z));\n// u_map2.addAccumulatedPoints(plane);\n// }\n// }\n// u_map.update();\n// u_map2.update();\n//\n// EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[0], u_map2.polynomialAt(0, 0).coefficients()[0]);\n// EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[1], u_map2.polynomialAt(0, 0).coefficients()[1]);\n// EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[2], u_map2.polynomialAt(0, 0).coefficients()[2]);\n//}\n\nint main(int argc,\n char **argv)\n{\n testing::InitGoogleTest(&argc, argv);\n return RUN_ALL_TESTS();\n}\n" } ]