Showing preview only (250K chars total). Download the full file or copy to clipboard to get everything.
Repository: iaslab-unipd/rgbd_calibration
Branch: indigo
Commit: 1839fce1a00d
Files: 37
Total size: 237.6 KB
Directory structure:
gitextract_t12mjzuf/
├── CMakeLists.txt
├── README.md
├── conf/
│ ├── checkerboards.yaml
│ ├── checkerboards_pepper.yaml
│ └── sim_camera.yaml
├── include/
│ └── rgbd_calibration/
│ ├── calibration.h
│ ├── calibration_node.h
│ ├── calibration_test.h
│ ├── checkerboard_views.h
│ ├── checkerboard_views_extractor.h
│ ├── depth_undistortion_estimation.h
│ ├── globals.h
│ ├── offline_calibration_node.h
│ ├── plane_based_extrinsic_calibration.h
│ ├── publisher.h
│ ├── simulation_node.h
│ └── test_node.h
├── launch/
│ ├── kinect2_507_tro.launch
│ ├── kinect2_507_tro_test.launch
│ ├── kinect_47A_tro.launch
│ ├── kinect_47A_tro_test.launch
│ ├── kinect_data_collection.launch
│ └── pepper_tro.launch
├── msg/
│ └── Acquisition.msg
├── package.xml
├── src/
│ └── rgbd_calibration/
│ ├── calibration.cpp
│ ├── calibration_node.cpp
│ ├── calibration_test.cpp
│ ├── checkerboard_views.cpp
│ ├── checkerboard_views_extractor.cpp
│ ├── data_collection_node.cpp
│ ├── depth_undistortion_estimation.cpp
│ ├── offline_calibration_node.cpp
│ ├── publisher.cpp
│ ├── simulation_node.cpp
│ └── test_node.cpp
└── test/
└── polynomial_undistortion_matrix_multifit_test.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(rgbd_calibration)
set(CMAKE_BUILD_TYPE RelWithDebInfo)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS cmake_modules roscpp calibration_common geometry_msgs kinect
eigen_conversions camera_info_manager cv_bridge pcl_ros
image_transport)# swissranger_camera)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(Boost REQUIRED)
find_package(Eigen REQUIRED)
find_package(PCL 1.7 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(OpenMP REQUIRED)
find_package(Ceres REQUIRED)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
## Uncomment this if the package has a setup.py. This macro ensures
## modules and scripts declared therein get installed
# catkin_python_setup()
#######################################
## Declare ROS messages and services ##
#######################################
## Generate messages in the 'msg' folder
add_message_files(
FILES
Acquisition.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
###################################################
## Declare things to be passed to other projects ##
###################################################
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES interactive_checkerboard_finder rgbd_calibration
CATKIN_DEPENDS roscpp calibration_common geometry_msgs kinect
eigen_conversions camera_info_manager cv_bridge pcl_ros image_transport
DEPENDS eigen pcl opencv2
)
###########
## Build ##
###########
## Specify additional locations of header files
include_directories(include
${catkin_INCLUDE_DIRS}
${EIGEN_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
)
## Declare a cpp library
add_library(rgbd_calibration
src/rgbd_calibration/calibration.cpp include/rgbd_calibration/calibration.h
include/rgbd_calibration/globals.h
src/rgbd_calibration/depth_undistortion_estimation.cpp include/rgbd_calibration/depth_undistortion_estimation.h
src/rgbd_calibration/checkerboard_views.cpp include/rgbd_calibration/checkerboard_views.h
src/rgbd_calibration/checkerboard_views_extractor.cpp include/rgbd_calibration/checkerboard_views_extractor.h
src/rgbd_calibration/publisher.cpp include/rgbd_calibration/publisher.h
)
add_executable(rgbd_offline_calibration
src/rgbd_calibration/calibration_node.cpp include/rgbd_calibration/calibration_node.h
src/rgbd_calibration/offline_calibration_node.cpp include/rgbd_calibration/offline_calibration_node.h
)
#add_executable(simulation
# src/rgbd_calibration/simulation_node.cpp include/rgbd_calibration/simulation_node.h
#)
add_executable(test_calibration
src/rgbd_calibration/test_node.cpp include/rgbd_calibration/test_node.h
src/rgbd_calibration/calibration_test.cpp include/rgbd_calibration/calibration_test.h
)
add_executable(data_collection
src/rgbd_calibration/data_collection_node.cpp
)
## Add dependencies to the executable
# add_dependencies(calibration_node ${PROJECT_NAME})
## Specify libraries to link a library or executable target against
target_link_libraries(rgbd_calibration
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBS}
${CERES_LIBRARIES}
)
target_link_libraries(rgbd_offline_calibration
rgbd_calibration
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBS}
${CERES_LIBRARIES}
)
#target_link_libraries(simulation
# rgbd_calibration
# ${catkin_LIBRARIES}
# ${PCL_LIBRARIES}
# ${OpenCV_LIBS}
# ${CERES_LIBRARIES}
#)
target_link_libraries(test_calibration
rgbd_calibration
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBS}
${CERES_LIBRARIES}
)
target_link_libraries(data_collection
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBS}
)
#############
## Install ##
#############
## Mark executable scripts (Python etc.) for installation
## not required for python when using catkin_python_setup()
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS calibration calibration_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/polynomial_undistortion_matrix_multifit_test.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test
# ${catkin_LIBRARIES}
# ${PCL_LIBRARIES}
# ${OpenCV_LIBS}
# ${CERES_LIBRARIES})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
================================================
FILE: README.md
================================================
## Dependencies
This package depends on [calibration_toolkit](https://github.com/iaslab-unipd/calibration_toolkit) version 0.3.2 (i.e. `git checkout v0.3.2`).
## Initial Setup
Edit **conf/checkerboards.yaml** to match your checkerboard(s) parameters.
## Data Collection
Use the [sensor_data_collection](https://github.com/iaslab-unipd/sensor_data_collection) package for collecting data from your sensors.
## Calibration
File **launch/kinect_47A_tro.launch** is a sample file to run the offline calibration of a Kinect 1.
Modify it to match your setup and perform the calibration.
================================================
FILE: conf/checkerboards.yaml
================================================
checkerboards:
#- rows: 7
# cols: 6
# cell_width: 0.095
# cell_height: 0.096
- rows: 5
cols: 6
cell_width: 0.12
cell_height: 0.12
#- rows: 10
# cols: 9
# cell_width: 0.058
# cell_height: 0.0583
#- rows: 5
# cols: 8
# cell_width: 0.030
# cell_height: 0.030
================================================
FILE: conf/checkerboards_pepper.yaml
================================================
checkerboards:
- rows: 7
cols: 9
cell_width: 0.02
cell_height: 0.02
================================================
FILE: conf/sim_camera.yaml
================================================
image_width: 640
image_height: 480
camera_name: sim_camera
camera_matrix:
rows: 3
cols: 3
data: [ 525, 0, 320, 0, 525, 240, 0, 0, 1 ]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [ 0, 0, 0, 0, 0 ]
rectification_matrix:
rows: 3
cols: 3
data: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ]
projection_matrix:
rows: 3
cols: 4
data: [ 525, 0, 320, 0, 0, 525, 240, 0, 0, 0, 1, 0 ]
================================================
FILE: include/rgbd_calibration/calibration.h
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RGBD_CALIBRATION_CALIBRATION_H_
#define RGBD_CALIBRATION_CALIBRATION_H_
#include <calibration_common/pinhole/sensor.h>
#include <kinect/depth/sensor.h>
#include <rgbd_calibration/depth_undistortion_estimation.h>
#include <rgbd_calibration/publisher.h>
#include <rgbd_calibration/checkerboard_views.h>
#include <rgbd_calibration/plane_based_extrinsic_calibration.h>
namespace calibration
{
class Calibration
{
public:
typedef boost::shared_ptr<Calibration> Ptr;
typedef boost::shared_ptr<const Calibration> ConstPtr;
inline void
setColorSensor (const PinholeSensor::Ptr & color_sensor)
{
color_sensor_ = color_sensor;
}
inline void
setDepthSensor (const KinectDepthSensor<UndistortionModel>::Ptr & depth_sensor)
{
depth_sensor_ = depth_sensor;
depth_intrinsics_.resize(4);
depth_intrinsics_[0] = depth_sensor->cameraModel()->intrinsicMatrix()(0, 0);
depth_intrinsics_[1] = depth_sensor->cameraModel()->intrinsicMatrix()(1, 1);
depth_intrinsics_[2] = depth_sensor->cameraModel()->intrinsicMatrix()(0, 2);
depth_intrinsics_[3] = depth_sensor->cameraModel()->intrinsicMatrix()(1, 2);
}
const std::vector<double> &
optimizedIntrinsics () const
{
return depth_intrinsics_;
}
inline void
setCheckerboards (const std::vector<Checkerboard::ConstPtr> & cb_vec)
{
cb_vec_ = cb_vec;
}
inline void
setPublisher (const Publisher::Ptr & publisher)
{
publisher_ = publisher;
}
inline void
setDownSampleRatio (int ratio)
{
assert(ratio > 0);
ratio_ = ratio;
}
void
addData (const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud);
// void
// addTestData (const cv::Mat & image,
// const PCLCloud3::ConstPtr & cloud);
inline void
setEstimateInitialTransform (bool estimate_initial_trasform)
{
estimate_initial_trasform_ = estimate_initial_trasform;
}
void
initDepthUndistortionModel ()
{
assert(local_matrix_ and global_matrix_);
estimate_depth_und_model_ = true;
depth_undistortion_estimation_ = boost::make_shared<DepthUndistortionEstimation>();
depth_undistortion_estimation_->setDepthErrorFunction(depth_sensor_->depthErrorFunction());
//depth_undistortion_estimation_->setDepthErrorFunction(Polynomial<Scalar, 2, 0>(Vector3(0.000, 0.000, 0.0035)));
depth_undistortion_estimation_->setLocalModel(local_model_);
depth_undistortion_estimation_->setGlobalModel(global_model_);
depth_undistortion_estimation_->setMaxThreads(8);
}
inline void
addCheckerboardViews (const CheckerboardViews::Ptr & rgbd_cb)
{
cb_views_vec_.push_back(rgbd_cb);
}
inline void
setLocalModel (const LocalModel::Ptr & model)
{
local_model_ = model;
local_matrix_ = boost::make_shared<LocalMatrixPCL>(local_model_);
}
const LocalModel::Ptr &
localModel () const
{
return local_model_;
}
inline void
setGlobalModel (const GlobalModel::Ptr & model)
{
global_model_ = model;
global_matrix_ = boost::make_shared<GlobalMatrixPCL>(global_model_);
}
const GlobalModel::Ptr &
globalModel () const
{
return global_model_;
}
void
perform();
void
optimize();
void
publishData() const;
protected:
void
estimateInitialTransform ();
void
estimateTransform (const std::vector<CheckerboardViews::Ptr> & rgbd_cb_vec);
void
optimizeTransform (const std::vector<CheckerboardViews::Ptr> & rgbd_cb_vec);
void
optimizeAll (const std::vector<CheckerboardViews::Ptr> & rgbd_cb_vec);
void
addData_ (const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud,
std::vector<RGBDData::ConstPtr> & vec);
PinholeSensor::Ptr color_sensor_;
KinectDepthSensor<UndistortionModel>::Ptr depth_sensor_;
std::vector<Checkerboard::ConstPtr> cb_vec_;
Publisher::Ptr publisher_;
bool estimate_depth_und_model_;
bool estimate_initial_trasform_;
int ratio_;
LocalModel::Ptr local_model_;
GlobalModel::Ptr global_model_;
LocalMatrixPCL::Ptr local_matrix_;
GlobalMatrixPCL::Ptr global_matrix_;
DepthUndistortionEstimation::Ptr depth_undistortion_estimation_;
std::vector<RGBDData::ConstPtr> data_vec_;
std::vector<RGBDData::ConstPtr> test_vec_;
std::vector<CheckerboardViews::Ptr> cb_views_vec_;
std::vector<DepthUndistortionEstimation::DepthData::Ptr> depth_data_vec_;
std::vector<double> depth_intrinsics_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_CALIBRATION_H_ */
================================================
FILE: include/rgbd_calibration/calibration_node.h
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RGBD_CALIBRATION_CALIBRATION_NODE_H_
#define RGBD_CALIBRATION_CALIBRATION_NODE_H_
#include <ros/ros.h>
#include <calibration_msgs/CheckerboardArray.h>
#include <calibration_common/base/base.h>
#include <calibration_common/objects/checkerboard.h>
#include <kinect/depth/sensor.h>
#include <rgbd_calibration/globals.h>
#include <rgbd_calibration/calibration.h>
namespace calibration
{
class CalibrationNode
{
public:
CalibrationNode (ros::NodeHandle & node_handle);
virtual
~CalibrationNode ()
{
// Do nothing
}
virtual bool
initialize ();
virtual void
spin () = 0;
static Checkerboard::Ptr
createCheckerboard (const calibration_msgs::CheckerboardMsg::ConstPtr & msg,
int id);
static Checkerboard::Ptr
createCheckerboard (const calibration_msgs::CheckerboardMsg & msg,
int id);
protected:
void
checkerboardArrayCallback (const calibration_msgs::CheckerboardArray::ConstPtr & msg);
bool
waitForMessages () const;
ros::NodeHandle node_handle_;
// TODO find another way to get checkerboards
ros::Subscriber checkerboards_sub_;
calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;
std::vector<Checkerboard::ConstPtr> cb_vec_;
std::string camera_calib_url_;
std::string camera_name_;
std::string depth_camera_calib_url_;
std::string depth_camera_name_;
bool has_initial_transform_;
Transform initial_transform_;
int downsample_ratio_;
Size2 undistortion_matrix_cell_size_;
Size2 images_size_;
Calibration::Ptr calibration_;
Publisher::Ptr publisher_;
PinholeSensor::Ptr color_sensor_;
KinectDepthSensor<UndistortionModel>::Ptr depth_sensor_;
std::vector<double> depth_error_coeffs_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_CALIBRATION_NODE_H_ */
================================================
FILE: include/rgbd_calibration/calibration_test.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_CALIBRATION_TEST_H_
#define RGBD_CALIBRATION_CALIBRATION_TEST_H_
#include <calibration_common/pinhole/sensor.h>
#include <kinect/depth/sensor.h>
#include <rgbd_calibration/globals.h>
#include <rgbd_calibration/publisher.h>
#include <rgbd_calibration/checkerboard_views.h>
//#define HERRERA
//#define UNCALIBRATED
//#define SKEW
namespace calibration
{
class CalibrationTest
{
public:
typedef boost::shared_ptr<CalibrationTest> Ptr;
typedef boost::shared_ptr<const CalibrationTest> ConstPtr;
void setColorSensor(const PinholeSensor::Ptr & color_sensor)
{
color_sensor_ = color_sensor;
}
void setDepthSensor(const KinectDepthSensorBase::Ptr & depth_sensor)
{
depth_sensor_ = depth_sensor;
}
void setCheckerboards(const std::vector<Checkerboard::ConstPtr> & cb_vec)
{
cb_vec_ = cb_vec;
}
void setPublisher(const Publisher::Ptr & publisher)
{
publisher_ = publisher;
}
void setDownSampleRatio(int ratio)
{
assert(ratio > 0);
ratio_ = ratio;
}
PCLCloudRGB::Ptr addData(const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud);
void setLocalModel(const LocalModel::Ptr & model)
{
local_matrix_ = boost::make_shared<LocalMatrixPCL>(model);
}
void setGlobalModel(const GlobalModel::Ptr & model)
{
global_matrix_ = boost::make_shared<GlobalMatrixPCL>(model);
}
void publishData() const;
void visualizeClouds() const;
void testPlanarityError() const;
void testCheckerboardError() const;
void testCube() const;
protected:
boost::shared_ptr<std::vector<int> >
extractPlaneFromCloud (const PCLCloud3::Ptr & cloud,
const Cloud2 & depth_corners) const;
PinholeSensor::Ptr color_sensor_;
KinectDepthSensorBase::Ptr depth_sensor_;
std::vector<Checkerboard::ConstPtr> cb_vec_;
Publisher::Ptr publisher_;
int ratio_;
LocalMatrixPCL::Ptr local_matrix_;
GlobalMatrixPCL::Ptr global_matrix_;
public:
std::vector<RGBDData::ConstPtr> data_vec_;
std::vector<RGBDData::ConstPtr> part_und_data_vec_;
std::vector<RGBDData::ConstPtr> und_data_vec_;
std::map<RGBDData::ConstPtr, RGBDData::ConstPtr> data_map_;
std::map<RGBDData::ConstPtr, RGBDData::ConstPtr> part_data_map_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_CALIBRATION_TEST_H_ */
================================================
FILE: include/rgbd_calibration/checkerboard_views.h
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_
#define RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_
#include <calibration_common/pinhole/view.h>
#include <calibration_common/depth/view.h>
#include <calibration_common/objects/checkerboard.h>
#include <calibration_common/algorithms/plane_extraction.h>
#include <calibration_common/rgbd/data.h>
#include <pcl/pcl_base.h>
namespace calibration
{
class CheckerboardViews
{
public:
typedef boost::shared_ptr<CheckerboardViews> Ptr;
typedef boost::shared_ptr<const CheckerboardViews> ConstPtr;
CheckerboardViews(const std::string & id)
: id_(id)
{
// Do nothing
}
inline void setId(const std::string & id)
{
id_ = id;
}
inline const std::string & id() const
{
return id_;
}
inline void setData(const RGBDData::ConstPtr & data)
{
data_ = data;
}
inline const RGBDData::ConstPtr & data() const
{
return data_;
}
inline void setCheckerboard(const Checkerboard::ConstPtr & checkerboard)
{
checkerboard_ = checkerboard;
}
inline const Checkerboard::ConstPtr & checkerboard() const
{
return checkerboard_;
}
inline const Checkerboard::Ptr & colorCheckerboard() const
{
return color_checkerboard_;
}
inline const PlanarObject::Ptr & depthPlane() const
{
return depth_plane_;
}
inline PinholeView<Checkerboard>::ConstPtr colorView() const
{
return color_view_;
}
inline DepthViewPCL<PlanarObject>::ConstPtr depthView() const
{
return depth_view_;
}
inline const pcl::IndicesConstPtr & planeInliers() const
{
return plane_inliers_;
}
void setColorView(const PinholeView<Checkerboard>::ConstPtr & color_view);
void setImageCorners(const Cloud2 & image_corners);
void setPlaneInliers(const pcl::IndicesConstPtr & plane_inliers,
Scalar inliers_std_dev);
void setPlaneInliers(const PlaneInfo & plane_info);
void draw(cv::Mat & image) const;
protected:
std::string id_;
RGBDData::ConstPtr data_;
Checkerboard::ConstPtr checkerboard_;
PinholeView<Checkerboard>::Ptr color_view_;
DepthViewPCL<PlanarObject>::Ptr depth_view_;
Checkerboard::Ptr color_checkerboard_;
PlanarObject::Ptr depth_plane_;
pcl::IndicesConstPtr plane_inliers_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_CHECKERBOARD_VIEWS_H_ */
================================================
FILE: include/rgbd_calibration/checkerboard_views_extractor.h
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_
#define RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_
#include <vector>
#include <rgbd_calibration/checkerboard_views.h>
namespace calibration
{
class CheckerboardViewsExtraction
{
public:
typedef ColorView<PinholeSensor, Checkerboard> CheckerboardView;
typedef DepthViewEigen<PlanarObject> PlaneView;
CheckerboardViewsExtraction()
: force_(false),
only_images_(false),
color_sensor_pose_(Pose::Identity()),
cb_constraint_(boost::make_shared<NoConstraint<Checkerboard> >()),
plane_constraint_(boost::make_shared<NoConstraint<PlanarObject> >())
{
}
inline void setCheckerboardVector(const std::vector<Checkerboard::ConstPtr> & cb_vec)
{
cb_vec_ = cb_vec;
}
inline void addCheckerboard(const Checkerboard::ConstPtr & checkerboard)
{
cb_vec_.push_back(checkerboard);
}
inline void setCheckerboardConstraint(const Constraint<Checkerboard>::ConstPtr & cb_constraint)
{
cb_constraint_ = cb_constraint;
}
inline void setPlanarObjectConstraint(const Constraint<PlanarObject>::ConstPtr & plane_constraint)
{
plane_constraint_ = plane_constraint;
}
inline void setInputData(const RGBDData::ConstPtr & data)
{
data_ = data;
}
inline void setInputData(const std::vector<RGBDData::ConstPtr> & data_vec)
{
data_vec_ = data_vec;
}
inline void setForceAll(bool force)
{
force_ = force;
}
inline void setOnlyImages(bool only_images)
{
only_images_ = only_images;
}
inline void setColorSensorPose(const Pose & color_sensor_pose)
{
color_sensor_pose_ = color_sensor_pose;
}
Size1 extract(std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive = false) const;
Size1 extractAll(std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive = false) const;
private:
Size1 extract(const RGBDData::ConstPtr & data,
std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive,
bool force) const;
std::vector<Checkerboard::ConstPtr> cb_vec_;
RGBDData::ConstPtr data_;
std::vector<RGBDData::ConstPtr> data_vec_;
bool force_;
bool only_images_;
Pose color_sensor_pose_;
Constraint<Checkerboard>::ConstPtr cb_constraint_;
Constraint<PlanarObject>::ConstPtr plane_constraint_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_CHECKERBOARD_VIEWS_EXTRACTOR_H_ */
================================================
FILE: include/rgbd_calibration/depth_undistortion_estimation.h
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_
#define RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_
#include <pcl/pcl_base.h>
#include <calibration_common/objects/checkerboard.h>
#include <calibration_common/algorithms/plane_extraction.h>
#include <calibration_common/depth/undistortion_model_fit.h>
#include <rgbd_calibration/globals.h>
#include <rgbd_calibration/publisher.h>
namespace calibration
{
class DepthUndistortionEstimation
{
public:
struct DepthData
{
typedef boost::shared_ptr<DepthData> Ptr;
typedef boost::shared_ptr<const DepthData> ConstPtr;
DepthData(int id)
: id_(id),
plane_extracted_(false)
{
}
int id_;
PCLCloud3::ConstPtr cloud_;
Checkerboard::ConstPtr checkerboard_; // Attention: in depth coordinates!!
PCLCloud3::ConstPtr undistorted_cloud_;
PlaneInfo estimated_plane_;
bool plane_extracted_;
};
struct OrderByDistance
{
inline bool operator()(const DepthData::Ptr & lhs,
const DepthData::Ptr & rhs)
{
return lhs->checkerboard_->corners().container().row(2).maxCoeff()
< rhs->checkerboard_->corners().container().row(2).maxCoeff();
}
};
typedef boost::shared_ptr<DepthUndistortionEstimation> Ptr;
typedef boost::shared_ptr<const DepthUndistortionEstimation> ConstPtr;
DepthUndistortionEstimation()
: max_threads_(1)
{
// Do nothing
}
inline void setLocalModel(const LocalModel::Ptr & local_model)
{
local_model_ = local_model;
local_fit_ = boost::make_shared<LocalMatrixFitPCL>(local_model_);
local_fit_->setDepthErrorFunction(depth_error_function_);
}
inline void setGlobalModel(const GlobalModel::Ptr & global_model)
{
global_model_ = global_model;
global_fit_ = boost::make_shared<GlobalMatrixFitPCL>(global_model_);
global_fit_->setDepthErrorFunction(depth_error_function_);
InverseGlobalModel::Data::Ptr matrix = boost::make_shared<InverseGlobalModel::Data>(Size2(1, 1), InverseGlobalPolynomial::IdentityCoefficients());
inverse_global_model_ = boost::make_shared<InverseGlobalModel>(global_model_->imageSize());
inverse_global_model_->setMatrix(matrix);
inverse_global_fit_ = boost::make_shared<InverseGlobalMatrixFitEigen>(inverse_global_model_);
}
inline void addDepthData(const DepthData::Ptr & data)
{
data_vec_.push_back(data);
}
inline const DepthData::Ptr & addDepthData(const PCLCloud3::ConstPtr & cloud,
const Checkerboard::ConstPtr & checkerboard)
{
DepthData::Ptr data = boost::make_shared<DepthData>(data_vec_.size() + 1);
data->cloud_ = cloud;
data->checkerboard_ = checkerboard;
addDepthData(data);
return data_vec_.back();
}
inline void setDepthData(const std::vector<DepthData::Ptr> & data_vec)
{
data_vec_ = data_vec;
}
void estimateLocalModel();
void estimateLocalModelReverse();
void optimizeLocalModel(const Polynomial<double, 2> & depth_error_function);
void estimateGlobalModel();
inline void setMaxThreads(size_t max_threads)
{
assert(max_threads > 0);
max_threads_ = max_threads;
}
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> getLocalSamples(Size1 x_index,
Size1 y_index) const
{
const LocalMatrixFitPCL::DataBin & samples = local_fit_->getSamples(x_index, y_index);
Eigen::Matrix<Scalar, Eigen::Dynamic, 3> eigen_samples(samples.size(), 3);
for (Size1 i = 0; i < samples.size(); ++i)
{
const LocalMatrixFitPCL::Data & sample = samples[i];
eigen_samples.row(i) << sample.x_, sample.y_, sample.weight_;
}
return eigen_samples;
}
Eigen::Matrix<Scalar, Eigen::Dynamic, 3> getGlobalSamples(Size1 x_index,
Size1 y_index) const
{
const GlobalMatrixFitPCL::DataBin & samples = global_fit_->getSamples(x_index, y_index);
Eigen::Matrix<Scalar, Eigen::Dynamic, 3> eigen_samples(samples.size(), 3);
for (Size1 i = 0; i < samples.size(); ++i)
{
const GlobalMatrixFitPCL::Data & sample = samples[i];
eigen_samples.row(i) << sample.x_, sample.y_, sample.weight_;
}
return eigen_samples;
}
void setDepthErrorFunction(const Polynomial<Scalar, 2> & depth_error_function)
{
depth_error_function_ = depth_error_function;
}
private:
bool extractPlane(const Checkerboard & color_cb,
const PCLCloud3::ConstPtr & cloud,
const Point3 & color_cb_center,
PlaneInfo & plane_info);
Size1 max_threads_;
LocalModel::Ptr local_model_;
LocalMatrixFitPCL::Ptr local_fit_;
GlobalModel::Ptr global_model_;
GlobalMatrixFitPCL::Ptr global_fit_;
InverseGlobalModel::Ptr inverse_global_model_;
InverseGlobalMatrixFitEigen::Ptr inverse_global_fit_;
std::vector<DepthData::Ptr> data_vec_;
std::map<DepthData::ConstPtr, PlaneInfo> plane_info_map_;
Polynomial<Scalar, 2> depth_error_function_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_DEPTH_UNDISTORTION_ESTIMATION_H_ */
================================================
FILE: include/rgbd_calibration/globals.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_GLOBALS_H_
#define RGBD_CALIBRATION_GLOBALS_H_
#include <calibration_common/base/math.h>
#include <calibration_common/objects/globals.h>
#include <calibration_common/depth/depth.h>
//#include <kinect/depth/polynomial_function_fit.h>
#include <kinect/depth/polynomial_matrix_fit.h>
#include <kinect/depth/two_steps_undistortion.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#define G_POLY_DEGREE 2
#define G_POLY_MIN_DEGREE 1
#define L_POLY_DEGREE 2
#define L_POLY_MIN_DEGREE 0
//#define KINECT_FOV_X 58.6 //Scalar(DEG2RAD(57.0)) 58.6
//#define KINECT_FOV_Y 45.7 //Scalar(DEG2RAD(43.5)) 45.7
//#define KINECT_ERROR_POLY Vector3(0.0, 0.0, 0.0035)
namespace calibration
{
typedef Polynomial<Scalar, L_POLY_DEGREE, L_POLY_MIN_DEGREE> LocalPolynomial;
typedef Polynomial<Scalar, G_POLY_DEGREE, G_POLY_MIN_DEGREE> GlobalPolynomial;
typedef Polynomial<Scalar, G_POLY_DEGREE, 0> InverseGlobalPolynomial;
typedef PolynomialMatrixSmoothModel<LocalPolynomial> LocalModel;
typedef PolynomialMatrixPCL<LocalModel, Scalar, PCLPoint3> LocalMatrixPCL;
typedef PolynomialMatrixEigen<LocalModel, Scalar> LocalMatrixEigen;
typedef PolynomialMatrixSmoothModelFitPCL<LocalPolynomial, Scalar, PCLPoint3> LocalMatrixFitPCL;
typedef PolynomialMatrixSmoothModelFitEigen<LocalPolynomial, Scalar> LocalMatrixFitEigen;
typedef PolynomialMatrixSmoothModel<GlobalPolynomial> GlobalModel;
typedef PolynomialMatrixPCL<GlobalModel, Scalar, PCLPoint3> GlobalMatrixPCL;
typedef PolynomialMatrixEigen<GlobalModel, Scalar> GlobalMatrixEigen;
typedef PolynomialMatrixSmoothModelFitPCL<GlobalPolynomial, Scalar, PCLPoint3> GlobalMatrixFitPCL;
typedef PolynomialMatrixSmoothModelFitEigen<GlobalPolynomial, Scalar> GlobalMatrixFitEigen;
typedef PolynomialMatrixSimpleModel<InverseGlobalPolynomial> InverseGlobalModel;
typedef PolynomialMatrixPCL<InverseGlobalModel, Scalar, PCLPoint3> InverseGlobalMatrixPCL;
typedef PolynomialMatrixEigen<InverseGlobalModel, Scalar> InverseGlobalMatrixEigen;
typedef PolynomialMatrixSimpleModelFitPCL<InverseGlobalPolynomial, Scalar, PCLPoint3> InverseGlobalMatrixFitPCL;
typedef PolynomialMatrixSimpleModelFitEigen<InverseGlobalPolynomial, Scalar> InverseGlobalMatrixFitEigen;
//typedef PolynomialFunctionModel<GlobalPolynomial>::Data UFunctionData;
//typedef PolynomialFunctionPCL<GlobalPolynomial, PCLPoint3> UFunctionPCL;
//typedef PolynomialFunctionEigen<GlobalPolynomial, Scalar> UFunctionEigen;
//typedef PolynomialFunctionFitPCL<GlobalPolynomial, Scalar, PCLPoint3> UFunctionFitPCL;
//typedef PolynomialFunctionFitEigen<GlobalPolynomial, Scalar> UFunctionFitEigen;
typedef TwoStepsModel<Scalar, LocalModel, GlobalModel> UndistortionModel;
typedef TwoStepsUndistortionEigen<Scalar, LocalModel, GlobalModel> UndistortionEigen;
typedef TwoStepsUndistortionPCL<Scalar, PCLPoint3, LocalModel, GlobalModel> UndistortionPCL;
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_GLOBALS_H_ */
================================================
FILE: include/rgbd_calibration/offline_calibration_node.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_
#define RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_
#include <rgbd_calibration/calibration_node.h>
namespace calibration
{
enum DepthType
{
KINECT1_DEPTH,
SWISS_RANGER_DEPTH
};
class OfflineCalibrationNode : public CalibrationNode
{
public:
OfflineCalibrationNode (ros::NodeHandle & node_handle);
virtual
~OfflineCalibrationNode()
{
// Do nothing
}
virtual void
spin ();
protected:
int instances_;
int starting_index_;
std::string path_;
std::string image_extension_;
std::string image_filename_;
std::string cloud_filename_;
DepthType depth_type_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_OFFLINE_CALIBRATION_NODE_H_ */
================================================
FILE: include/rgbd_calibration/plane_based_extrinsic_calibration.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_
#define RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_
#include <calibration_common/algorithms/plane_to_plane_calibration.h>
#include <calibration_common/objects/sensor.h>
#include <calibration_common/objects/planar_object.h>
namespace calibration
{
class PlaneBasedExtrinsicCalibration
{
public:
typedef boost::shared_ptr<PlaneBasedExtrinsicCalibration> Ptr;
typedef boost::shared_ptr<const PlaneBasedExtrinsicCalibration> ConstPtr;
void addData(size_t index,
const Sensor::Ptr & sensor,
const PlanarObject::ConstPtr & data)
{
assert(index < data_map_.size());
data_map_[index][sensor] = data;
}
void setSize(size_t size)
{
data_map_.resize(size);
}
size_t size() const
{
return data_map_.size();
}
size_t appendData(const std::map<Sensor::Ptr, PlanarObject::ConstPtr> & data)
{
data_map_.push_back(data);
return data_map_.size();
}
void setMainSensor(const Sensor::Ptr & world)
{
world_ = world;
}
void perform()
{
std::map<Sensor::Ptr, PlaneToPlaneCalibration> calib_map;
for (size_t i = 0; i < data_map_.size(); ++i)
{
std::map<Sensor::Ptr, PlanarObject::ConstPtr> & data_map = data_map_[i];
if (data_map.find(world_) == data_map.end())
continue;
const PlanarObject::ConstPtr & world_data = data_map_[i][world_];
for (std::map<Sensor::Ptr, PlanarObject::ConstPtr>::const_iterator it = data_map.begin(); it != data_map.end(); ++it)
{
const Sensor::Ptr & sensor = it->first;
const PlanarObject::ConstPtr & sensor_data = it->second;
if (sensor != world_)
calib_map[sensor].addPair(world_data->plane(), sensor_data->plane());
}
}
for (std::map<Sensor::Ptr, PlaneToPlaneCalibration>::iterator it = calib_map.begin(); it != calib_map.end(); ++it)
{
const Sensor::Ptr & sensor = it->first;
if (calib_map[sensor].getPairNumber() > 5)
{
sensor->setParent(world_);
sensor->setPose(calib_map[sensor].estimateTransform());
}
else
sensor->setParent(Sensor::ConstPtr());
}
}
void optimize()
{
// TODO implement!!
}
protected:
Sensor::Ptr world_;
std::vector<std::map<Sensor::Ptr, PlanarObject::ConstPtr> > data_map_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_PLANE_BASED_EXTRINSIC_CALIBRATION_H_ */
================================================
FILE: include/rgbd_calibration/publisher.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_PUBLISHER_H_
#define RGBD_CALIBRATION_PUBLISHER_H_
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <rgbd_calibration/checkerboard_views.h>
namespace calibration
{
class Publisher
{
private:
struct CheckerboardPublisherSet
{
ros::Publisher marker_pub_;
};
struct DataPublisherSet
{
ros::Publisher cloud_pub_;
ros::Publisher rgbd_pub_;
};
public:
typedef boost::shared_ptr<Publisher> Ptr;
typedef boost::shared_ptr<const Publisher> ConstPtr;
Publisher(const ros::NodeHandle & node_handle);
void publish(const CheckerboardViews & rgbd,
const std::string & prefix = "");
void publish(const RGBDData & rgbd);
void publish(const Cloud3 & point_set,
const BaseObject & frame);
void publishTF(const BaseObject & object);
private:
ros::NodeHandle node_handle_;
ros::Publisher marker_pub_;
tf::TransformBroadcaster tf_pub_;
std::map<int, DataPublisherSet> d_pub_map_;
std::map<std::string, CheckerboardPublisherSet> c_pub_map_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_PUBLISHER_H_ */
================================================
FILE: include/rgbd_calibration/simulation_node.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_SIMULATION_NODE_H_
#define RGBD_CALIBRATION_SIMULATION_NODE_H_
#include <rgbd_calibration/calibration_node.h>
namespace calibration
{
class SimulationNode : public CalibrationNode
{
public:
SimulationNode(ros::NodeHandle & node_handle);
virtual void spin();
protected:
double depth_error_;
double image_error_;
double min_distance_;
double max_distance_;
double step_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_SIMULATION_NODE_H_ */
================================================
FILE: include/rgbd_calibration/test_node.h
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef RGBD_CALIBRATION_TEST_NODE_H_
#define RGBD_CALIBRATION_TEST_NODE_H_
#include <ros/ros.h>
#include <calibration_msgs/CheckerboardArray.h>
#include <calibration_common/base/base.h>
#include <calibration_common/objects/checkerboard.h>
#include <kinect/depth/sensor.h>
#include <rgbd_calibration/globals.h>
#include <rgbd_calibration/calibration_test.h>
namespace calibration
{
class TestNode
{
public:
enum DepthType
{
KINECT1_DEPTH,
SWISS_RANGER_DEPTH
};
TestNode(ros::NodeHandle & node_handle);
virtual ~TestNode()
{
// Do nothing
}
virtual bool initialize();
virtual void spin();
virtual void spin2();
virtual void spin3();
static Checkerboard::Ptr createCheckerboard(const calibration_msgs::CheckerboardMsg::ConstPtr & msg,
int id);
static Checkerboard::Ptr createCheckerboard(const calibration_msgs::CheckerboardMsg & msg,
int id);
protected:
void checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg);
bool waitForMessages() const;
ros::NodeHandle node_handle_;
// TODO find another way to get checkerboards
ros::Subscriber checkerboards_sub_;
calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;
std::vector<Checkerboard::ConstPtr> cb_vec_;
std::string camera_calib_url_;
std::string camera_name_;
std::string depth_camera_calib_url_;
std::string depth_camera_name_;
Pose camera_pose_;
std::string local_matrix_file_;
std::string global_matrix_file_;
int downsample_ratio_;
PinholeSensor::Ptr color_sensor_;
KinectDepthSensor<UndistortionModel>::Ptr depth_sensor_;
Publisher::Ptr publisher_;
std::vector<Scalar> depth_intrinsics_;
int instances_;
int starting_index_;
std::string path_;
std::string image_extension_;
std::string image_filename_;
std::string cloud_filename_;
bool only_show_;
CalibrationTest::Ptr test_;
Size2 images_size_;
DepthType depth_type_;
};
} /* namespace calibration */
#endif /* RGBD_CALIBRATION_TEST_NODE_H_ */
================================================
FILE: launch/kinect2_507_tro.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="ns" default="calibration" />
<group ns="$(arg ns)">
<arg name="path" default="$(env HOME)/Desktop/dataset/kinect2/" />
<arg name="instances" default="1000" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="1" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="point_cloud_" />
<arg name="camera_name" default="rgb_507663142542" />
<arg name="camera_type" default="pinhole" />
<arg name="camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg camera_name).yaml" />
<arg name="depth_camera_name" default="depth_507663142542" />
<arg name="depth_type" default="kinect1_depth" />
<arg name="depth_camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg depth_camera_name).yaml" />
<arg name="downsample_ratio" default="2" />
<!-- launch-prefix="gdb -ex run - -args" -->
<node pkg="rgbd_calibration" type="rgbd_offline_calibration" name="rgbd_offline" output="screen" required="true">
<param name="path" value="$(arg path)" />
<param name="instances" value="$(arg instances)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_name" value="$(arg camera_name)" />
<param name="camera_type" value="$(arg camera_type)" />
<param name="camera_calib_url" value="$(arg camera_calib_url)" />
<param name="depth_camera_name" value="$(arg depth_camera_name)" />
<param name="depth_type" value="$(arg depth_type)" />
<param name="depth_camera_calib_url" value="$(arg depth_camera_calib_url)" />
<rosparam>
camera_pose:
translation: {x: 0.0, y: 0.0, z: 0.0}
rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
depth_error_function: [0.002, -0.001, 0.002]
depth_image:
cols: 512
rows: 424
undistortion_matrix:
cell_size_x: 2
cell_size_y: 2
</rosparam>
<param name="downsample_ratio" value="$(arg downsample_ratio)" />
</node>
<node pkg="rostopic" type="rostopic" name="checkerboards_pub" output="screen"
args="pub -f $(find rgbd_calibration)/conf/checkerboards.yaml rgbd_offline/checkerboard_array
calibration_msgs/CheckerboardArray --latch">
</node>
</group>
</launch>
================================================
FILE: launch/kinect2_507_tro_test.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="ns" default="calibration" />
<arg name="dir" default="$(env HOME)/Desktop/dataset/kinect2" />
<group ns="$(arg ns)">
<arg name="path" default="$(env HOME)/Desktop/dataset_cubo/kinect2/" />
<arg name="instances" default="1000" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="1" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="point_cloud_" />
<arg name="camera_name" default="rgb_507663142542" />
<arg name="camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg camera_name).yaml" />
<arg name="depth_camera_name" default="depth_507663142542" />
<arg name="depth_type" default="kinect1_depth" />
<arg name="depth_camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg depth_camera_name).yaml" />
<arg name="downsample_ratio" default="1" />
<arg name="local_und_matrix_file" default="$(arg dir)/local_matrix.txt" />
<arg name="global_und_matrix_file" default="$(arg dir)/global_matrix.txt" />
<arg name="only_show" default="false" />
<node pkg="rgbd_calibration" type="test_calibration" name="test_calibration" output="screen" required="true">
<param name="path" value="$(arg path)" />
<param name="instances" value="$(arg instances)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_name" value="$(arg camera_name)" />
<param name="camera_calib_url" value="$(arg camera_calib_url)" />
<param name="depth_type" value="$(arg depth_type)" />
<param name="depth_camera_name" value="$(arg depth_camera_name)" />
<param name="depth_camera_calib_url" value="$(arg depth_camera_calib_url)" />
<rosparam param="camera_pose" command="load" file="$(arg dir)/camera_pose.yaml" />
<param name="downsample_ratio" value="$(arg downsample_ratio)" />
<param name="local_und_matrix_file" value="$(arg local_und_matrix_file)" />
<param name="global_und_matrix_file" value="$(arg global_und_matrix_file)" />
<rosparam command="load" file="$(arg dir)/depth_intrinsics.yaml" />
<param name="only_show" value="$(arg only_show)" />
<rosparam>
depth_image:
cols: 512
rows: 424
</rosparam>
</node>
<node pkg="rostopic" type="rostopic" name="checkerboards_pub" output="screen"
args="pub -f $(find rgbd_calibration)/conf/checkerboards.yaml test_calibration/checkerboard_array
calibration_msgs/CheckerboardArray --latch">
</node>
</group>
</launch>
================================================
FILE: launch/kinect_47A_tro.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="ns" default="calibration" />
<group ns="$(arg ns)">
<arg name="path" default="$(env HOME)/Desktop/dataset/kinect1/" />
<arg name="instances" default="1000" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="1" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="point_cloud_" />
<arg name="camera_name" default="rgb_A00367A01433047A" />
<arg name="camera_type" default="pinhole" />
<arg name="camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg camera_name).yaml" />
<arg name="depth_camera_name" default="depth_A00367A01433047A" />
<arg name="depth_type" default="kinect1_depth" />
<arg name="depth_camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg depth_camera_name).yaml" />
<arg name="downsample_ratio" default="2" />
<!-- launch-prefix="gdb -ex run - -args" -->
<node pkg="rgbd_calibration" type="rgbd_offline_calibration" name="rgbd_offline" output="screen" required="true">
<param name="path" value="$(arg path)" />
<param name="instances" value="$(arg instances)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_name" value="$(arg camera_name)" />
<param name="camera_type" value="$(arg camera_type)" />
<param name="camera_calib_url" value="$(arg camera_calib_url)" />
<param name="depth_camera_name" value="$(arg depth_camera_name)" />
<param name="depth_type" value="$(arg depth_type)" />
<param name="depth_camera_calib_url" value="$(arg depth_camera_calib_url)" />
<rosparam>
camera_pose:
translation: {x: 0.0, y: 0.0, z: 0.0}
rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
depth_error_function: [-0.00029, 0.00037, 0.001365]
undistortion_matrix:
cell_size_x: 2
cell_size_y: 2
</rosparam>
<!-- [0.0019, -0.0016, 0.0020] [0.00172, -0.00120, 0.00142]-->
<param name="downsample_ratio" value="$(arg downsample_ratio)" />
</node>
<node pkg="rostopic" type="rostopic" name="checkerboards_pub" output="screen"
args="pub -f $(find rgbd_calibration)/conf/checkerboards.yaml rgbd_offline/checkerboard_array
calibration_msgs/CheckerboardArray --latch">
</node>
</group>
</launch>
================================================
FILE: launch/kinect_47A_tro_test.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="ns" default="calibration" />
<arg name="dir" default="$(env HOME)/Desktop/dataset/kinect1" />
<group ns="$(arg ns)">
<arg name="path" default="/home/filippo/Desktop/visual_odometry3/" />
<arg name="instances" default="1000" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="1" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="cloud_" />
<arg name="camera_name" default="rgb_A00367A01433047A" />
<arg name="camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg camera_name).yaml" />
<arg name="depth_camera_name" default="depth_A00367A01433047A" />
<arg name="depth_type" default="kinect1_depth" />
<arg name="depth_camera_calib_url" default="file://$(env HOME)/.ros/camera_info/$(arg depth_camera_name).yaml" />
<arg name="downsample_ratio" default="1" />
<arg name="local_und_matrix_file" default="$(arg dir)/local_matrix.txt" />
<arg name="global_und_matrix_file" default="$(arg dir)/global_matrix.txt" />
<arg name="only_show" default="false" />
<node pkg="rgbd_calibration" type="test_calibration" name="test_calibration" output="screen" required="true">
<param name="path" value="$(arg path)" />
<param name="instances" value="$(arg instances)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_name" value="$(arg camera_name)" />
<param name="camera_calib_url" value="$(arg camera_calib_url)" />
<param name="depth_type" value="$(arg depth_type)" />
<param name="depth_camera_name" value="$(arg depth_camera_name)" />
<param name="depth_camera_calib_url" value="$(arg depth_camera_calib_url)" />
<rosparam param="camera_pose" command="load" file="$(arg dir)/camera_pose.yaml" />
<param name="downsample_ratio" value="$(arg downsample_ratio)" />
<param name="local_und_matrix_file" value="$(arg local_und_matrix_file)" />
<param name="global_und_matrix_file" value="$(arg global_und_matrix_file)" />
<rosparam command="load" file="$(arg dir)/depth_intrinsics.yaml" />
<param name="only_show" value="$(arg only_show)" />
</node>
<node pkg="rostopic" type="rostopic" name="checkerboards_pub" output="screen"
args="pub -f $(find rgbd_calibration)/conf/checkerboards.yaml test_calibration/checkerboard_array
calibration_msgs/CheckerboardArray --latch">
</node>
</group>
</launch>
================================================
FILE: launch/kinect_data_collection.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="save_folder" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="1" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="cloud_" />
<arg name="search_checkerboard" default="false" />
<arg name="save_image" default="true" />
<arg name="save_camera_info" default="true" />
<arg name="save_depth_image" default="true" />
<arg name="save_depth_camera_info" default="true" />
<arg name="save_point_cloud" default="false" />
<arg name="camera_type" default="pinhole" />
<arg name="kinect_name" default="kinect1" />
<include file="$(find openni_launch)/launch/openni.launch">
<arg name="camera" value="$(arg kinect_name)" />
<arg name="depth_registration" value="false" />
<arg name="publish_tf" value="false" />
</include>
<node pkg="rgbd_calibration" type="data_collection" name="data_collection_kinect1" output="screen" required="true">
<param name="save_folder" value="$(arg save_folder)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_type" value="$(arg camera_type)" />
<param name="search_checkerboard" value="$(arg search_checkerboard)" />
<param name="save_image" value="$(arg save_image)" />
<param name="save_image_camera_info" value="$(arg save_camera_info)" />
<param name="save_depth_image" value="$(arg save_depth_image)" />
<param name="save_depth_camera_info" value="$(arg save_depth_camera_info)" />
<param name="save_point_cloud" value="$(arg save_point_cloud)" />
<param name="depth_type" value="float32" />
<remap from="~action" to="/action" />
<remap from="~image" to="/$(arg kinect_name)/rgb/image_color" />
<remap from="~camera_info" to="/$(arg kinect_name)/rgb/camera_info" />
<remap from="~depth_image" to="/$(arg kinect_name)/depth/image" />
<remap from="~depth_camera_info" to="/$(arg kinect_name)/depth/camera_info" />
<remap from="~point_cloud" to="/$(arg kinect_name)/depth/points" />
</node>
</launch>
================================================
FILE: launch/pepper_tro.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="ns" default="calibration" />
<group ns="$(arg ns)">
<arg name="path" default="/home/filippo/Downloads/images/pepper/" />
<arg name="instances" default="1000" />
<arg name="image_extension" default="png" />
<arg name="starting_index" default="10" />
<arg name="image_filename" default="image_" />
<arg name="cloud_filename" default="point_cloud_" />
<arg name="camera_name" default="pepper_front" />
<arg name="camera_type" default="pinhole" />
<arg name="camera_calib_url" default="file:///home/filippo/Downloads/images/pepper/camera_info.yaml"/>
<arg name="depth_camera_name" default="pepper_depth" />
<arg name="depth_type" default="kinect1_depth" />
<arg name="depth_camera_calib_url" default="file:///home/filippo/Downloads/images/pepper/depth_info.yaml" />
<arg name="downsample_ratio" default="1" />
<!-- launch-prefix="gdb -ex run - -args" -->
<node pkg="rgbd_calibration" type="rgbd_offline_calibration" name="rgbd_offline" output="screen" required="true">
<param name="path" value="$(arg path)" />
<param name="instances" value="$(arg instances)" />
<param name="image_extension" value="$(arg image_extension)" />
<param name="starting_index" value="$(arg starting_index)" />
<param name="image_filename" value="$(arg image_filename)" />
<param name="cloud_filename" value="$(arg cloud_filename)" />
<param name="camera_name" value="$(arg camera_name)" />
<param name="camera_type" value="$(arg camera_type)" />
<param name="camera_calib_url" value="$(arg camera_calib_url)" />
<param name="depth_camera_name" value="$(arg depth_camera_name)" />
<param name="depth_type" value="$(arg depth_type)" />
<param name="depth_camera_calib_url" value="$(arg depth_camera_calib_url)" />
<rosparam>
camera_pose:
translation: {x: -0.039, y: 0.044, z: -0.035}
rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
depth_image:
cols: 320
rows: 240
depth_error_function: [-0.0029, 0.0037, 0.01365]
undistortion_matrix:
cell_size_x: 2
cell_size_y: 2
</rosparam>
<!-- [0.0019, -0.0016, 0.0020] [0.00172, -0.00120, 0.00142]-->
<param name="downsample_ratio" value="$(arg downsample_ratio)" />
</node>
<node pkg="rostopic" type="rostopic" name="checkerboards_pub" output="screen"
args="pub -f $(find rgbd_calibration)/conf/checkerboards_pepper.yaml rgbd_offline/checkerboard_array
calibration_msgs/CheckerboardArray --latch">
</node>
</group>
</launch>
================================================
FILE: msg/Acquisition.msg
================================================
float32 distance
string info
================================================
FILE: package.xml
================================================
<?xml version="1.0"?>
<package>
<name>rgbd_calibration</name>
<version>0.0.0</version>
<description>The calibration package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="filippo@todo.todo">filippo</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://ros.org/wiki/calibration</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>calibration_common</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>camera_info_manager</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>kinect</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>calibration_common</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>camera_info_manager</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>kinect</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
FILE: src/rgbd_calibration/calibration.cpp
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <omp.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/random_sample.h>
#include <pcl/io/pcd_io.h>
#include <eigen_conversions/eigen_msg.h>
#include <calibration_common/ceres/plane_fit.h>
#include <calibration_common/base/pcl_conversion.h>
#include <kinect/depth/polynomial_matrix_io.h>
#include <rgbd_calibration/checkerboard_views_extractor.h>
#include <rgbd_calibration/plane_based_extrinsic_calibration.h>
#include <rgbd_calibration/depth_undistortion_estimation.h>
#include <rgbd_calibration/calibration.h>
#define RGBD_INFO(id, msg) ROS_INFO_STREAM("RGBD " << id << ": " << msg)
namespace calibration
{
class CheckerboardDistanceConstraint : public Constraint<Checkerboard>
{
public:
CheckerboardDistanceConstraint (Scalar distance,
const Point3 & from = Point3::Zero())
: distance_(distance),
from_(from)
{
// Do nothing
}
virtual
~CheckerboardDistanceConstraint()
{
// Do nothing
}
inline virtual bool
isValid (const Checkerboard & checkerboard) const
{
return (checkerboard.center() - from_).norm() <= distance_;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
Scalar distance_;
Point3 from_;
};
void
Calibration::publishData () const
{
if (not publisher_)
return;
publisher_->publishTF(*depth_sensor_);
publisher_->publishTF(*color_sensor_);
for (size_t i = 0; i < test_vec_.size(); ++i)
publisher_->publish(*test_vec_[i]);
for (size_t i = 0; i < cb_views_vec_.size(); ++i)
publisher_->publish(*cb_views_vec_[i]);
}
void
Calibration::addData_ (const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud,
std::vector<RGBDData::ConstPtr> & vec)
{
RGBDData::Ptr data(boost::make_shared<RGBDData>(data_vec_.size() + 1));
data->setColorSensor(color_sensor_);
data->setDepthSensor(depth_sensor_);
//cv::Mat rectified;
//color_sensor_->cameraModel()->rectifyImage(image, rectified);
data->setColorData(image);
if (ratio_ > 1)
{
PCLCloud3::Ptr new_cloud(boost::make_shared<PCLCloud3>());
new_cloud->resize(cloud->size() / (ratio_ * ratio_));
new_cloud->header = cloud->header;
new_cloud->width = cloud->width / ratio_;
new_cloud->height = cloud->height / ratio_;
new_cloud->is_dense = cloud->is_dense;
PCLPoint3 zero(0, 0, 0);
float nan = std::numeric_limits<float>::quiet_NaN();
PCLPoint3 bad_point(nan, nan, nan);
for (Size1 i = 0; i < new_cloud->height; ++i)
{
for (Size1 j = 0; j < new_cloud->width; ++j)
{
new_cloud->at(j, i) = zero;
int count = 0;
for (Size1 di = 0; di < ratio_; ++di)
{
for (Size1 dj = 0; dj < ratio_; ++dj)
{
const PCLPoint3 & p = cloud->at(j * ratio_ + dj, i * ratio_ + di);
if (pcl::isFinite(p))
{
++count;
new_cloud->at(j, i).x += p.x;
new_cloud->at(j, i).y += p.y;
new_cloud->at(j, i).z += p.z;
}
}
}
if (count > 0)
{
new_cloud->at(j, i).x /= count;
new_cloud->at(j, i).y /= count;
new_cloud->at(j, i).z /= count;
}
else
{
new_cloud->at(j, i) = bad_point;
new_cloud->is_dense = false;
}
}
}
data->setDepthData(*new_cloud);
}
else
{
data->setDepthData(*cloud);
}
vec.push_back(data);
}
void
Calibration::addData (const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud)
{
addData_(image, cloud, data_vec_);
}
//void
//Calibration::addTestData (const cv::Mat & image,
// const PCLCloud3::ConstPtr & cloud)
//{
// addData_(image, cloud, test_vec_);
//}
void
Calibration::perform ()
{
if (estimate_initial_trasform_ or not color_sensor_->parent())
estimateInitialTransform();
if (estimate_depth_und_model_)
{
CheckerboardViewsExtraction cb_extractor;
cb_extractor.setColorSensorPose(color_sensor_->pose());
cb_extractor.setCheckerboardVector(cb_vec_);
cb_extractor.setInputData(data_vec_);
cb_extractor.setOnlyImages(true);
cb_extractor.extractAll(cb_views_vec_);
ROS_INFO_STREAM(cb_views_vec_.size());
for (Size1 i = 0; i < cb_views_vec_.size(); ++i)
{
CheckerboardViews & cb_views = *cb_views_vec_[i];
Checkerboard::Ptr cb = boost::make_shared<Checkerboard>(*cb_views.colorCheckerboard());
cb->transform(color_sensor_->pose());
depth_data_vec_.push_back(depth_undistortion_estimation_->addDepthData(cb_views.data()->depthData(), cb));
}
ROS_INFO_STREAM("Estimating undistortion map...");
depth_undistortion_estimation_->estimateLocalModel();
ROS_INFO_STREAM("Recomputing undistortion map...");
depth_undistortion_estimation_->estimateLocalModelReverse();
ROS_INFO_STREAM("Estimating global error correction map...");
depth_undistortion_estimation_->estimateGlobalModel();
for (Size1 i = 0; i < cb_views_vec_.size(); ++i)
{
CheckerboardViews & cb_views = *cb_views_vec_[i];
if (depth_data_vec_[i]->plane_extracted_)
cb_views.setPlaneInliers(depth_data_vec_[i]->estimated_plane_);
else
cb_views_vec_[i].reset();
}
// PolynomialUndistortionMatrixIO<LocalPolynomial> io;
//// io.write(*local_matrix_->model(), "/tmp/local_matrix.txt");
// int index = 0;
// for (Scalar i = 1.0; i < 5.5; i += 0.125, ++index)
// {
// Scalar max;
// std::stringstream ss;
// ss << "/tmp/matrix_"<< index << ".png";
// io.writeImageAuto(*local_matrix_->model(), i, ss.str(), max);
// ROS_INFO_STREAM("Max " << i << ": " << max);
// }
// std::vector<std::pair<int, int> > pixels;
// pixels.push_back(std::make_pair(0, 0));
// pixels.push_back(std::make_pair(40/ratio_/local_model_->binSize().x(), 480/ratio_/local_model_->binSize().y()));
// pixels.push_back(std::make_pair(320/ratio_/local_model_->binSize().x(), 240/ratio_/local_model_->binSize().y()));
// pixels.push_back(std::make_pair(560/ratio_/local_model_->binSize().x(), 400/ratio_/local_model_->binSize().y()));
//
// for (int i = 0; i < pixels.size(); ++i)
// {
// std::cout << "Local Polynomial at (" << pixels[i].first << ", " << pixels[i].second << "): " << local_model_->polynomial(pixels[i].first, pixels[i].second).transpose() << std::endl;
//
// std::stringstream ss;
// ss << "/tmp/local_samples_" << pixels[i].first << "_" << pixels[i].second << ".txt";
//
// std::fstream fs;
// fs.open(ss.str().c_str(), std::fstream::out);
// fs << depth_undistortion_estimation_->getLocalSamples(pixels[i].first, pixels[i].second) << std::endl;
// fs.close();
// }
//
// for (int i = 0; i < 4; ++i)
// {
// std::cout << "Global Polynomial at (" << i / 2 << ", " << i % 2 << "): " << global_model_->polynomial(i / 2, i % 2).transpose() << std::endl;
//
// std::stringstream ss;
// ss << "/tmp/global_samples_" << i / 2 << "_" << i % 2 << ".txt";
//
// std::fstream fs;
// fs.open(ss.str().c_str(), std::fstream::out);
// fs << depth_undistortion_estimation_->getGlobalSamples(i / 2, i % 2) << std::endl;
// fs.close();
// }
}
estimateTransform(cb_views_vec_);
}
void
Calibration::estimateInitialTransform ()
{
CheckerboardViewsExtraction cb_extractor;
cb_extractor.setCheckerboardVector(cb_vec_);
cb_extractor.setCheckerboardConstraint(boost::make_shared<CheckerboardDistanceConstraint>(2.0));
std::vector<CheckerboardViews::Ptr> cb_views_vec;
for (Size1 i = 0; i < data_vec_.size() and cb_views_vec.size() < 10; ++i)
{
Size1 index = rand() % data_vec_.size();
cb_extractor.setInputData(data_vec_[index]);
cb_extractor.extract(cb_views_vec, true);
}
estimateTransform(cb_views_vec);
}
void
Calibration::estimateTransform (const std::vector<CheckerboardViews::Ptr> & cb_views_vec)
{
PlaneBasedExtrinsicCalibration calib;
calib.setMainSensor(depth_sensor_);
calib.setSize(cb_views_vec.size());
int index = 0;
for (Size1 i = 0; i < cb_views_vec.size(); ++i)
{
if (cb_views_vec[i])
{
calib.addData(index, color_sensor_, cb_views_vec[i]->colorCheckerboard());
calib.addData(index++, depth_sensor_, cb_views_vec[i]->depthPlane());
}
}
calib.setSize(index);
calib.perform();
}
class TransformError
{
public:
TransformError(const PinholeCameraModel::ConstPtr & camera_model,
const Checkerboard::ConstPtr & checkerboard,
const Cloud2 & image_corners,
const Plane & depth_plane,
const Polynomial<Scalar, 2> & depth_error_function)
: camera_model_(camera_model),
checkerboard_(checkerboard),
image_corners_(image_corners),
depth_plane_(depth_plane),
depth_error_function_(depth_error_function)
{
}
template <typename T>
bool operator ()(const T * const color_sensor_pose,
const T * const checkerboard_pose,
T * residuals) const
{
typename Types<T>::Vector3 color_sensor_r_vec(color_sensor_pose[0], color_sensor_pose[1], color_sensor_pose[2]);
typename Types<T>::AngleAxis color_sensor_r(color_sensor_r_vec.norm(), color_sensor_r_vec.normalized());
typename Types<T>::Translation3 color_sensor_t(color_sensor_pose[3], color_sensor_pose[4], color_sensor_pose[5]);
typename Types<T>::Transform color_sensor_pose_eigen = color_sensor_t * color_sensor_r;
typename Types<T>::Vector3 checkerboard_r_vec(checkerboard_pose[0], checkerboard_pose[1], checkerboard_pose[2]);
typename Types<T>::AngleAxis checkerboard_r(checkerboard_r_vec.norm(), checkerboard_r_vec.normalized());
typename Types<T>::Translation3 checkerboard_t(checkerboard_pose[3], checkerboard_pose[4], checkerboard_pose[5]);
typename Types<T>::Transform checkerboard_pose_eigen = checkerboard_t * checkerboard_r;
typename Types<T>::Cloud3 cb_corners(checkerboard_->corners().size());
cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast<T>();
typename Types<T>::Plane depth_plane(depth_plane_.normal().cast<T>(), T(depth_plane_.offset()));
typename Types<T>::Cloud2 reprojected_corners = camera_model_->project3dToPixel2<T>(cb_corners);
cb_corners.transform(color_sensor_pose_eigen);
Polynomial<T, 2> depth_error_function(depth_error_function_.coefficients().cast<T>());
for (Size1 i = 0; i < cb_corners.elements(); ++i)
{
residuals[2 * i] = T((reprojected_corners[i] - image_corners_[i].cast<T>()).norm() / 0.5);
residuals[2 * i + 1] = T(depth_plane.absDistance(cb_corners[i])
/ ceres::poly_eval(depth_error_function.coefficients(), cb_corners[i].z())); // TODO use line-of-sight error
}
return true;
}
private:
const PinholeCameraModel::ConstPtr & camera_model_;
const Checkerboard::ConstPtr & checkerboard_;
const Cloud2 & image_corners_;
const Plane & depth_plane_;
const Polynomial<Scalar, 2> depth_error_function_;
};
void Calibration::optimizeTransform(const std::vector<CheckerboardViews::Ptr> & cb_views_vec)
{
ceres::Problem problem;
Eigen::Matrix<Scalar, Eigen::Dynamic, 6, Eigen::DontAlign | Eigen::RowMajor> data(cb_views_vec.size(), 6);
AngleAxis rotation(color_sensor_->pose().linear());
Translation3 translation(color_sensor_->pose().translation());
Eigen::Matrix<Scalar, 1, 6, Eigen::DontAlign | Eigen::RowMajor> transform;
transform.head<3>() = rotation.angle() * rotation.axis();
transform.tail<3>() = translation.vector();
for (size_t i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
rotation = AngleAxis(cb_views.colorCheckerboard()->pose().linear());
data.row(i).head<3>() = rotation.angle() * rotation.axis();
data.row(i).tail<3>() = cb_views.colorCheckerboard()->pose().translation();
TransformError * error = new TransformError(color_sensor_->cameraModel(),
cb_views.checkerboard(),
cb_views.colorView()->points(),
cb_views.depthPlane()->plane(),
depth_sensor_->depthErrorFunction());
typedef ceres::AutoDiffCostFunction<TransformError, ceres::DYNAMIC, 6, 6> TransformCostFunction;
ceres::CostFunction * cost_function = new TransformCostFunction(error, 2 * cb_views.checkerboard()->size());
problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(1.0), transform.data(), data.row(i).data());
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.max_num_iterations = 100;
options.minimizer_progress_to_stdout = true;
options.num_threads = 8;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
rotation.angle() = transform.head<3>().norm();
rotation.axis() = transform.head<3>().normalized();
translation.vector() = transform.tail<3>();
color_sensor_->setPose(translation * rotation);
}
class ReprojectionError
{
public:
ReprojectionError (const PinholeCameraModel::ConstPtr & camera_model,
const Checkerboard::ConstPtr & checkerboard,
const Cloud2 & image_corners)
: camera_model_(camera_model),
checkerboard_(checkerboard),
image_corners_(image_corners)
{
// Do nothing
}
template <typename T>
typename Types<T>::Pose toEigen(const T * const q, const T * const t) const
{
typename Types<T>::Quaternion rotation(q[0], q[1], q[2], q[3]);
typename Types<T>::Translation3 translation(t[0], t[1], t[2]);
return translation * rotation;
}
template <typename T>
bool
operator () (const T * const checkerboard_pose_q,
const T * const checkerboard_pose_t,
T * residuals) const
{
typename Types<T>::Pose checkerboard_pose_eigen = toEigen<T>(checkerboard_pose_q, checkerboard_pose_t);
typename Types<T>::Cloud3 cb_corners(checkerboard_->corners().size());
cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast<T>();
typename Types<T>::Cloud2 reprojected_corners(checkerboard_->corners().size());
for (Size1 i = 0; i < cb_corners.elements(); ++i)
reprojected_corners[i] = camera_model_->project3dToPixel2<T>(cb_corners[i]);
Eigen::Map<Eigen::Matrix<T, 2, Eigen::Dynamic> > residual_map(residuals, 2, cb_corners.elements());
residual_map = (reprojected_corners.container() - image_corners_.container().cast<T>()) / (0.5 * std::sqrt(T(cb_corners.elements())));
return true;
}
private:
const PinholeCameraModel::ConstPtr & camera_model_;
const Checkerboard::ConstPtr & checkerboard_;
Cloud2 image_corners_;
};
class TransformDistortionError
{
public:
TransformDistortionError(const PinholeCameraModel::ConstPtr & camera_model,
const KinectDepthCameraModel::ConstPtr & depth_camera_model,
const Checkerboard::ConstPtr & checkerboard,
const Cloud3 & depth_points,
const Indices & plane_indices,
const Polynomial<Scalar, 2> & depth_error_function,
const Size2 & images_size)
: camera_model_(camera_model),
depth_camera_model_(depth_camera_model),
checkerboard_(checkerboard),
depth_points_(depth_points),
plane_indices_(plane_indices),
depth_error_function_(depth_error_function),
images_size_(images_size)
{
// Do nothing
}
template <typename T>
typename Types<T>::Pose toEigen(const T * const q, const T * const t) const
{
typename Types<T>::Quaternion rotation(q[0], q[1], q[2], q[3]);
typename Types<T>::Translation3 translation(t[0], t[1], t[2]);
return translation * rotation;
}
template <typename T>
bool operator ()(const T * const color_sensor_pose_q,
const T * const color_sensor_pose_t,
const T * const global_undistortion,
const T * const checkerboard_pose_q,
const T * const checkerboard_pose_t,
const T * const delta,
T * residuals) const
{
typename Types<T>::Pose color_sensor_pose_eigen = toEigen<T>(color_sensor_pose_q, color_sensor_pose_t);
typename Types<T>::Pose checkerboard_pose_eigen = toEigen<T>(checkerboard_pose_q, checkerboard_pose_t);
const int DEGREE = MathTraits<GlobalPolynomial>::Degree;
const int MIN_DEGREE = MathTraits<GlobalPolynomial>::MinDegree;
const int SIZE = MathTraits<GlobalPolynomial>::Size;//DEGREE - MIN_DEGREE + 1;
typedef MathTraits<GlobalPolynomial>::Coefficients Coefficients;
Size1 index = 0;
Coefficients c1, c2, c3;
for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)
c1[i] = global_undistortion[index++];
for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)
c2[i] = global_undistortion[index++];
for (Size1 i = 0 ; i < DEGREE - MIN_DEGREE + 1; ++i)
c3[i] = global_undistortion[index++];
Polynomial<T, DEGREE, MIN_DEGREE> p1(c1);
Polynomial<T, DEGREE, MIN_DEGREE> p2(c2);
Polynomial<T, DEGREE, MIN_DEGREE> p3(c3);
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> A(SIZE, SIZE);
Eigen::Matrix<T, Eigen::Dynamic, 1> b(SIZE, 1);
for (int i = 0; i < SIZE; ++i)
{
T x(i + 1);
T y = p2.evaluate(x) + p3.evaluate(x) - p1.evaluate(x);
T tmp(1.0);
for (int j = 0; j < MIN_DEGREE; ++j)
tmp *= x;
for (int j = 0; j < SIZE; ++j)
{
A(i, j) = tmp;
tmp *= x;
}
b[i] = y;
}
Eigen::Matrix<T, Eigen::Dynamic, 1> x = A.colPivHouseholderQr().solve(b);
GlobalModel::Data::Ptr global_data = boost::make_shared<GlobalModel::Data>(Size2(2, 2));
for (int i = 0; i < 3 * MathTraits<GlobalPolynomial>::Size; ++i)
global_data->container().data()[0 * MathTraits<GlobalPolynomial>::Size + i] = global_undistortion[i];
for (int i = 0; i < SIZE; ++i)
global_data->container().data()[3 * MathTraits<GlobalPolynomial>::Size + i] = x[i];
GlobalModel::Ptr global_model = boost::make_shared<GlobalModel>(images_size_);
global_model->setMatrix(global_data);
GlobalMatrixEigen global(global_model);
typename Types<T>::Cloud3 depth_points(depth_points_.size());
depth_points.container() = depth_points_.container().cast<T>();
const cv::Matx33d & K = depth_camera_model_->intrinsicMatrix();
for (int j = 0; j < depth_points.size().y(); ++j)
{
for (int i = 0; i < depth_points.size().x(); ++i)
{
T z = depth_points(i, j).z();
typename Types<T>::Point2 normalized_pixel((i - (K(0, 2) + delta[2])) / (K(0, 0) * delta[0]),
(j - (K(1, 2) + delta[3])) / (K(1, 1) * delta[1]));
depth_points(i, j) = z * depth_camera_model_->undistort2d_<T>(normalized_pixel).homogeneous();
/*T z = depth_points(i, j).z();
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]);
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]);*/
}
}
global.undistort(depth_points);
typename Types<T>::Cloud3 cb_corners(checkerboard_->corners().size());
cb_corners.container() = color_sensor_pose_eigen * checkerboard_pose_eigen * checkerboard_->corners().container().cast<T>();
Polynomial<T, 2> depth_error_function(depth_error_function_.coefficients().cast<T>());
typename Types<T>::Plane cb_plane = Types<T>::Plane::Through(cb_corners(0, 0), cb_corners(0, 1), cb_corners(1, 0));
Eigen::Map<Eigen::Matrix<T, 3, Eigen::Dynamic> > residual_map_dist(residuals, 3, plane_indices_.size());
for (Size1 i = 0; i < plane_indices_.size(); ++i)
{
Line line(Point3::Zero(), depth_points[plane_indices_[i]].normalized());
residual_map_dist.col(i) = (line.intersectionPoint(cb_plane) - depth_points[plane_indices_[i]]) /
(std::sqrt(T(plane_indices_.size())) * ceres::poly_eval(depth_error_function.coefficients(), depth_points[plane_indices_[i]].z()));
}
// Scalar alpha = std::acos(depth_plane.normal().dot(cb_plane.normal()));
// if (alpha > M_PI_2)
// alpha = M_PI - alpha;
// residual_map_dist *= std::exp(alpha);
return true;
}
private:
const PinholeCameraModel::ConstPtr & camera_model_;
const KinectDepthCameraModel::ConstPtr & depth_camera_model_;
const Checkerboard::ConstPtr & checkerboard_;
const Cloud3 depth_points_;
const Indices plane_indices_;
const Polynomial<Scalar, 2> depth_error_function_;
const Size2 images_size_;
};
typedef ceres::NumericDiffCostFunction<ReprojectionError, ceres::CENTRAL, ceres::DYNAMIC, 4, 3> ReprojectionCostFunction;
typedef ceres::NumericDiffCostFunction<TransformDistortionError, ceres::CENTRAL, ceres::DYNAMIC, 4, 3,
3 * MathTraits<GlobalPolynomial>::Size, 4, 3, 4> TransformDistortionCostFunction;
void
Calibration::optimizeAll (const std::vector<CheckerboardViews::Ptr> & cb_views_vec)
{
ceres::Problem problem;
Eigen::Matrix<Scalar, Eigen::Dynamic, 7, Eigen::DontAlign | Eigen::RowMajor> data(cb_views_vec.size(), 7);
//AngleAxis rotation(color_sensor_->pose().linear());
Quaternion rotation(color_sensor_->pose().linear());
Translation3 translation(color_sensor_->pose().translation());
Eigen::Matrix<Scalar, 1, 7 , Eigen::DontAlign | Eigen::RowMajor> transform;
//transform.head<3>() = rotation.angle() * rotation.axis();
transform[0] = rotation.w();
transform[1] = rotation.x();
transform[2] = rotation.y();
transform[3] = rotation.z();
transform.tail<3>() = translation.vector();
double delta[4] = {1.0, 1.0, 0.0, 0.0};
for (size_t i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
/*rotation = AngleAxis(cb_views.colorCheckerboard()->pose().linear());
data.row(i).head<3>() = rotation.angle() * rotation.axis();*/
Quaternion rotation = Quaternion(cb_views.colorCheckerboard()->pose().linear());
data.row(i)[0] = rotation.w();
data.row(i)[1] = rotation.x();
data.row(i)[2] = rotation.y();
data.row(i)[3] = rotation.z();
data.row(i).tail<3>() = cb_views.colorCheckerboard()->pose().translation();
TransformDistortionError * error;
ceres::CostFunction * cost_function;
error = new TransformDistortionError(color_sensor_->cameraModel(),
depth_sensor_->cameraModel(),
cb_views.checkerboard(),
PCLConversion<Scalar>::toPointMatrix(*cb_views.depthView()->data()),
cb_views.depthView()->points(),
depth_sensor_->depthErrorFunction(),
global_model_->imageSize());
cost_function = new TransformDistortionCostFunction(error,
ceres::DO_NOT_TAKE_OWNERSHIP,
3 * cb_views.depthView()->points().size());
problem.AddResidualBlock(cost_function,
NULL,//new ceres::CauchyLoss(1.0),
transform.data(),
transform.data() + 4,
global_matrix_->model()->dataPtr(),
data.row(i).data(),
data.row(i).data() + 4,
delta);
ReprojectionError * repr_error = new ReprojectionError(color_sensor_->cameraModel(),
cb_views.checkerboard(),
cb_views.colorView()->points());
ceres::CostFunction * repr_cost_function = new ReprojectionCostFunction(repr_error,
ceres::DO_NOT_TAKE_OWNERSHIP,
2 * cb_views.checkerboard()->size());
problem.AddResidualBlock(repr_cost_function,
NULL,//new ceres::CauchyLoss(1.0),
data.row(i).data(),
data.row(i).data() + 4);
problem.SetParameterization(data.row(i).data(), new ceres::QuaternionParameterization());
}
problem.SetParameterization(transform.data(), new ceres::QuaternionParameterization());
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 20;
options.minimizer_progress_to_stdout = true;
options.num_threads = 8;
// problem.SetParameterBlockConstant(delta);
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
rotation = Quaternion(transform[0], transform[1], transform[2], transform[3]);
translation.vector() = transform.tail<3>();
color_sensor_->setPose(translation * rotation);
const int DEGREE = MathTraits<GlobalPolynomial>::Degree;
const int MIN_DEGREE = MathTraits<GlobalPolynomial>::MinDegree;
const int SIZE = DEGREE - MIN_DEGREE + 1;
typedef MathTraits<GlobalPolynomial>::Coefficients Coefficients;
GlobalPolynomial p1(global_matrix_->model()->polynomial(0, 0));
GlobalPolynomial p2(global_matrix_->model()->polynomial(0, 1));
GlobalPolynomial p3(global_matrix_->model()->polynomial(1, 0));
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> A(SIZE, SIZE);
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> b(SIZE, 1);
for (int i = 0; i < SIZE; ++i)
{
Scalar x(i + 1);
Scalar y = p2.evaluate(x) + p3.evaluate(x) - p1.evaluate(x);
Scalar tmp(1.0);
for (int j = 0; j < MIN_DEGREE; ++j)
tmp *= x;
for (int j = 0; j < SIZE; ++j)
{
A(i, j) = tmp;
tmp *= x;
}
b[i] = y;
}
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> x = A.colPivHouseholderQr().solve(b);
global_matrix_->model()->polynomial(1, 1) = x;
depth_sensor_->cameraModel();
depth_intrinsics_[0] = depth_intrinsics_[0] * delta[0];
depth_intrinsics_[1] = depth_intrinsics_[1] * delta[1];
depth_intrinsics_[2] = depth_intrinsics_[2] + delta[2];
depth_intrinsics_[3] = depth_intrinsics_[3] + delta[3];
}
void
Calibration::optimize ()
{
ROS_INFO("Optimizing...\n");
if (estimate_depth_und_model_)
{
std::vector<CheckerboardViews::Ptr> und_cb_views_vec;
// Create locally undistorted clouds and views
#pragma omp parallel for
for (Size1 i = 0; i < cb_views_vec_.size(); ++i)
{
if (not cb_views_vec_[i])
continue;
const CheckerboardViews & cb_views = *cb_views_vec_[i];
const DepthUndistortionEstimation::DepthData & depth_data = *depth_data_vec_[i];
CheckerboardViews::Ptr und_cb_views = boost::make_shared<CheckerboardViews>(cb_views);
RGBDData::Ptr und_data = boost::make_shared<RGBDData>(*cb_views.data());
und_data->setDepthData(*depth_data.undistorted_cloud_);
// std::stringstream ss;
// ss << "/tmp/und_cloud_" << i << ".pcd";
// pcl::PCDWriter writer;
// writer.write(ss.str(), *depth_data.undistorted_cloud_);
//
// ss.str("");
// ss << "/tmp/cloud_" << i << ".pcd";
// writer.write(ss.str(), *depth_data.cloud_);
und_cb_views->setId(cb_views.id() + "_undistorted");
und_cb_views->setData(und_data);
und_cb_views->setPlaneInliers(depth_data.estimated_plane_.indices_, depth_data.estimated_plane_.std_dev_);
#pragma omp critical
und_cb_views_vec.push_back(und_cb_views);
}
optimizeAll(und_cb_views_vec);
}
else
{
optimizeTransform(cb_views_vec_);
}
// PolynomialUndistortionMatrixIO<GlobalPolynomial> io;
// //io.write(*global_matrix_->model(), "/tmp/opt_global_matrix.txt");
//
// int index = 0;
// for (Scalar i = 1.0; i < 5.5; i += 0.5, ++index)
// {
// Scalar max;
// std::stringstream ss;
// ss << "/tmp/g_matrix_"<< index << ".png";
// io.writeImageAuto(*global_matrix_->model(), i, ss.str(), max);
// ROS_INFO_STREAM("Max " << i << ": " << max);
// }
// const Size1 size = test_vec_.size();
//
// for (Size1 i = 0; i < size; ++i)
// {
// const RGBDData::ConstPtr & test_data = test_vec_[i];
//
// PCLCloud3::Ptr depth_data = boost::make_shared<PCLCloud3>(*test_data->depthData());
// UndistortionPCL und;
// und.setModel(depth_sensor_->undistortionModel());
// und.undistort(*depth_data);
// RGBDData::Ptr new_test_data = boost::make_shared<RGBDData>(*test_data);
// new_test_data->setDepthData(*depth_data);
// new_test_data->setId(10000 + test_data->id());
// test_vec_.push_back(new_test_data);
//
// }
// std::ofstream transform_file;
// transform_file.open("/tmp/camera_pose.yaml");
// geometry_msgs::Pose pose_msg;
// tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
// transform_file << pose_msg;
// transform_file.close();
}
} /* namespace calibration */
================================================
FILE: src/rgbd_calibration/calibration_node.cpp
================================================
#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <eigen_conversions/eigen_msg.h>
#include <calibration_common/pinhole/camera_model.h>
#include <camera_info_manager/camera_info_manager.h>
#include <kinect/depth/camera_model.h>
#include <rgbd_calibration/calibration_node.h>
using namespace camera_info_manager;
using namespace calibration_msgs;
namespace calibration
{
CalibrationNode::CalibrationNode (ros::NodeHandle & node_handle)
: node_handle_(node_handle),
publisher_(new Publisher(node_handle)),
has_initial_transform_(false)
{
checkerboards_sub_ = node_handle_.subscribe("checkerboard_array", 1, &CalibrationNode::checkerboardArrayCallback, this);
if (not node_handle_.getParam("camera_calib_url", camera_calib_url_))
ROS_FATAL("Missing \"camera_calib_url\" parameter!!");
if (not node_handle_.getParam("depth_camera_calib_url", depth_camera_calib_url_))
ROS_FATAL("Missing \"depth_camera_calib_url\" parameter!!");
node_handle_.param("camera_name", camera_name_, std::string("camera"));
node_handle_.param("depth_camera_name", depth_camera_name_, std::string("depth_camera"));
int undistortion_matrix_cell_size_x, undistortion_matrix_cell_size_y;
node_handle_.param("undistortion_matrix/cell_size_x", undistortion_matrix_cell_size_x, 8);
node_handle_.param("undistortion_matrix/cell_size_y", undistortion_matrix_cell_size_y, 8);
undistortion_matrix_cell_size_.x() = undistortion_matrix_cell_size_x;
undistortion_matrix_cell_size_.y() = undistortion_matrix_cell_size_y;
int images_size_x, images_size_y;
node_handle_.param("depth_image/cols", images_size_x, 640);
node_handle_.param("depth_image/rows", images_size_y, 480);
images_size_.x() = images_size_x;
images_size_.y() = images_size_y;
node_handle_.param("downsample_ratio", downsample_ratio_, 1);
if (downsample_ratio_ < 1)
{
downsample_ratio_ = 1;
ROS_WARN("\"downsample_ratio\" cannot be < 1. Skipping.");
}
else
{
images_size_ /= downsample_ratio_;
}
if (not node_handle_.getParam("depth_error_function", depth_error_coeffs_))
ROS_FATAL("Missing \"depth_error_function\" parameter!!");
else if (depth_error_coeffs_.size() != 3)
ROS_FATAL("\"depth_error_function\" must be a vector of size 3!!");
if (node_handle_.hasParam("camera_pose"))
{
Translation3 translation;
Quaternion rotation;
node_handle_.getParam("camera_pose/translation/x", translation.vector().coeffRef(0));
node_handle_.getParam("camera_pose/translation/y", translation.vector().coeffRef(1));
node_handle_.getParam("camera_pose/translation/z", translation.vector().coeffRef(2));
node_handle_.getParam("camera_pose/rotation/x", rotation.coeffs().coeffRef(0));
node_handle_.getParam("camera_pose/rotation/y", rotation.coeffs().coeffRef(1));
node_handle_.getParam("camera_pose/rotation/z", rotation.coeffs().coeffRef(2));
node_handle_.getParam("camera_pose/rotation/w", rotation.coeffs().coeffRef(3));
initial_transform_ = translation * rotation;
has_initial_transform_ = true;
}
}
bool
CalibrationNode::initialize ()
{
if (not waitForMessages())
return false;
calibration_ = boost::make_shared<Calibration>();
for (size_t i = 0; i < checkerboard_array_msg_->checkerboards.size(); ++i)
cb_vec_.push_back(createCheckerboard(checkerboard_array_msg_->checkerboards[i], i));
BaseObject::Ptr world = boost::make_shared<BaseObject>();
world->setFrameId("/world");
CameraInfoManager manager_depth(node_handle_, depth_camera_name_, depth_camera_calib_url_);
sensor_msgs::CameraInfo depth_camera_info = manager_depth.getCameraInfo();
depth_camera_info.binning_x = downsample_ratio_;
depth_camera_info.binning_y = downsample_ratio_;
KinectDepthCameraModel::Ptr depth_pinhole_model = boost::make_shared<KinectDepthCameraModel>(depth_camera_info);
std::cout << depth_pinhole_model->projectionMatrix() << std::endl;
depth_sensor_ = boost::make_shared<KinectDepthSensor<UndistortionModel> >();
depth_sensor_->setFrameId("/depth_sensor");
depth_sensor_->setParent(world);
depth_sensor_->setCameraModel(depth_pinhole_model);
Polynomial<Scalar, 2> depth_error_function(Vector3(depth_error_coeffs_[0], depth_error_coeffs_[1], depth_error_coeffs_[2]));
depth_sensor_->setDepthErrorFunction(depth_error_function);
CameraInfoManager manager(node_handle_, camera_name_, camera_calib_url_);
PinholeCameraModel::ConstPtr pinhole_model = boost::make_shared<PinholeCameraModel>(manager.getCameraInfo());
color_sensor_ = boost::make_shared<PinholeSensor>();
color_sensor_->setFrameId("/pinhole_sensor");
color_sensor_->setCameraModel(pinhole_model);
if (has_initial_transform_)
{
color_sensor_->setParent(depth_sensor_);
color_sensor_->transform(initial_transform_);
}
calibration_->setCheckerboards(cb_vec_);
calibration_->setDepthSensor(depth_sensor_);
calibration_->setColorSensor(color_sensor_);
if (not has_initial_transform_)
calibration_->setEstimateInitialTransform(true);
LocalModel::Ptr local_model = boost::make_shared<LocalModel>(images_size_);
LocalModel::Data::Ptr local_matrix = local_model->createMatrix(undistortion_matrix_cell_size_, LocalPolynomial::IdentityCoefficients());
local_model->setMatrix(local_matrix);
GlobalModel::Ptr global_model = boost::make_shared<GlobalModel>(images_size_);
GlobalModel::Data::Ptr global_data = boost::make_shared<GlobalModel::Data>(Size2(2, 2), GlobalPolynomial::IdentityCoefficients());
global_model->setMatrix(global_data);
UndistortionModel::Ptr model = boost::make_shared<UndistortionModel>();
model->setLocalModel(local_model);
model->setGlobalModel(global_model);
depth_sensor_->setUndistortionModel(model);
calibration_->setLocalModel(local_model);
calibration_->setGlobalModel(global_model);
calibration_->initDepthUndistortionModel();
calibration_->setPublisher(publisher_);
calibration_->setDownSampleRatio(downsample_ratio_);
return true;
}
void
CalibrationNode::checkerboardArrayCallback (const CheckerboardArray::ConstPtr & msg)
{
checkerboard_array_msg_ = msg;
}
bool
CalibrationNode::waitForMessages () const
{
ros::Rate rate(1.0);
ros::spinOnce();
while (ros::ok() and (not checkerboard_array_msg_))
{
ROS_WARN("Not all messages received!");
rate.sleep();
ros::spinOnce();
}
return checkerboard_array_msg_;
}
Checkerboard::Ptr
CalibrationNode::createCheckerboard (const CheckerboardMsg::ConstPtr & msg,
int id)
{
return createCheckerboard(*msg, id);
}
Checkerboard::Ptr
CalibrationNode::createCheckerboard (const CheckerboardMsg & msg,
int id)
{
std::stringstream ss;
ss << "/checkerboard_" << id;
Checkerboard::Ptr cb = boost::make_shared<Checkerboard>(msg.cols, msg.rows, msg.cell_width, msg.cell_height);
cb->setFrameId(ss.str());
return cb;
}
} /* namespace calibration */
================================================
FILE: src/rgbd_calibration/calibration_test.cpp
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <rgbd_calibration/calibration_test.h>
#include <rgbd_calibration/checkerboard_views_extractor.h>
#include <calibration_common/ceres/plane_fit.h>
#include <calibration_common/algorithms/automatic_checkerboard_finder.h>
#include <calibration_common/algorithms/interactive_checkerboard_finder.h>
#include <calibration_common/base/pcl_conversion.h>
#include <kinect/depth/polynomial_matrix_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
namespace calibration
{
void CalibrationTest::publishData() const
{
if (not publisher_)
return;
publisher_->publishTF(*depth_sensor_);
publisher_->publishTF(*color_sensor_);
for (size_t i = 0; i < data_vec_.size(); i += 1)
publisher_->publish(*data_vec_[i]);
for (size_t i = 0; i < und_data_vec_.size(); i += 1)
publisher_->publish(*und_data_vec_[i]);
}
PCLCloudRGB::Ptr CalibrationTest::addData(const cv::Mat & image,
const PCLCloud3::ConstPtr & cloud)
{
// PCLCloud3::Ptr new_cloud = boost::make_shared<PCLCloud3>();
// std::vector<int> remapping;
// pcl::removeNaNFromPointCloud(*cloud, *new_cloud, remapping);
PCLCloud3::Ptr new_cloud = boost::make_shared<PCLCloud3>(*cloud);
if (ratio_ > 1)
{
new_cloud = boost::make_shared<PCLCloud3>();
new_cloud->resize(cloud->size() / (ratio_ * ratio_));
new_cloud->header = cloud->header;
new_cloud->width = cloud->width / ratio_;
new_cloud->height = cloud->height / ratio_;
new_cloud->is_dense = cloud->is_dense;
PCLPoint3 zero(0, 0, 0);
float nan = std::numeric_limits<float>::quiet_NaN();
PCLPoint3 bad_point(nan, nan, nan);
for (Size1 i = 0; i < new_cloud->height; ++i)
{
for (Size1 j = 0; j < new_cloud->width; ++j)
{
new_cloud->at(j, i) = zero;
int count = 0;
for (Size1 di = 0; di < ratio_; ++di)
{
for (Size1 dj = 0; dj < ratio_; ++dj)
{
const PCLPoint3 & p = cloud->at(j * ratio_ + dj, i * ratio_ + di);
if (pcl::isFinite(p))
{
++count;
new_cloud->at(j, i).x += p.x;
new_cloud->at(j, i).y += p.y;
new_cloud->at(j, i).z += p.z;
}
}
}
if (count > 0)
{
new_cloud->at(j, i).x /= count;
new_cloud->at(j, i).y /= count;
new_cloud->at(j, i).z /= count;
}
else
{
new_cloud->at(j, i) = bad_point;
new_cloud->is_dense = false;
}
}
}
}
int index = data_vec_.size() + 1;
cv::Mat rectified = image;
//color_sensor_->cameraModel()->rectifyImage(image, rectified);
PinholeSensor::Ptr color_sensor_original = boost::make_shared<PinholeSensor>();
color_sensor_original->setFrameId("/pinhole_sensor_original");
color_sensor_original->setCameraModel(color_sensor_->cameraModel());
color_sensor_original->setParent(depth_sensor_);
color_sensor_original->transform(Transform::Identity() * Translation3(0.052, 0.0, 0.0));
RGBDData::Ptr data(boost::make_shared<RGBDData>(index));
data->setColorSensor(color_sensor_original);
data->setDepthSensor(depth_sensor_);
data->setColorData(rectified);
data->setDepthData(*new_cloud);
PCLCloud3::Ptr part_und_cloud = boost::make_shared<PCLCloud3>(*new_cloud);
local_matrix_->undistort(*part_und_cloud);
RGBDData::Ptr part_und_data(boost::make_shared<RGBDData>(index));
part_und_data->setColorSensor(color_sensor_);
part_und_data->setDepthSensor(depth_sensor_);
part_und_data->setColorData(rectified);
part_und_data->setDepthData(*part_und_cloud);
PCLCloud3::Ptr und_cloud = boost::make_shared<PCLCloud3>(*part_und_cloud);
global_matrix_->undistort(*und_cloud);
RGBDData::Ptr und_data(boost::make_shared<RGBDData>(index + 1000));
und_data->setColorSensor(color_sensor_);
und_data->setDepthSensor(depth_sensor_);
und_data->setColorData(rectified);
und_data->setDepthData(*und_cloud);
data_vec_.push_back(data);
part_und_data_vec_.push_back(part_und_data);
und_data_vec_.push_back(und_data);
part_data_map_[und_data] = part_und_data;
data_map_[und_data] = data;
und_data->fuseData();
return und_data->fusedData();
// float z = 0.0f;
// for (size_t i = 0; i < und_cloud->size(); ++i)
// z += und_cloud->points[i].z;
// z /= und_cloud->size();
// PCLCloud3::Ptr tmp_cloud = boost::make_shared<PCLCloud3>();
// pcl::PassThrough<pcl::PointXYZ> pass;
// pass.setInputCloud(cloud);
// pass.setFilterFieldName("y");
// pass.setFilterLimits(-10.0f, 10.0f);
// pass.filter(*tmp_cloud);
// pcl::VoxelGrid<pcl::PointXYZ> voxel;
// voxel.setInputCloud(tmp_cloud);
// voxel.setLeafSize(0.05f, 10.0f, 0.05f);
// voxel.filter(*tmp_cloud);
// std::fstream fs;
// fs.open("/tmp/points.txt", std::fstream::out | std::fstream::app);
// for (size_t i = 0; i < tmp_cloud->size(); ++i)
// fs << tmp_cloud->points[i].x << " " << tmp_cloud->points[i].z << " " << tmp_cloud->points[i].y << std::endl;
// fs << "------------------------------------------------------------------" << std::endl;
// fs.close();
// pass.setInputCloud(und_cloud);
// pass.filter(*tmp_cloud);
// voxel.setInputCloud(tmp_cloud);
// voxel.filter(*tmp_cloud);
// fs.open("/tmp/und_points.txt", std::fstream::out | std::fstream::app);
// for (size_t i = 0; i < tmp_cloud->size(); ++i)
// fs << tmp_cloud->points[i].x << " " << tmp_cloud->points[i].z << " " << tmp_cloud->points[i].y << std::endl;
// fs << "------------------------------------------------------------------" << std::endl;
// fs.close();
}
void CalibrationTest::visualizeClouds() const
{
pcl::visualization::PCLVisualizer viz1("Test Set Visualization");
PCLCloud3::Ptr fake_cloud = boost::make_shared<PCLCloud3>(640/ratio_, 480/ratio_);
const PinholeCameraModel & camera_model = *color_sensor_->cameraModel();
pcl::visualization::PointCloudColorHandlerGenericField<PCLPoint3> color_handler(fake_cloud, "z");
//viz1.addPointCloud(fake_cloud, color_handler);
ros::Rate rate = ros::Rate(100.0);
for (float s = 1.0f; s <= 4.5f; s += 1.5f)
{
for (size_t i = 0; i < fake_cloud->width; ++i)
{
for (size_t j = 0; j < fake_cloud->height; ++j)
{
PCLPoint3 & fake_p = fake_cloud->at(i, j);
fake_p.x = (i - camera_model.cx()) * s / camera_model.fx();
fake_p.y = (j - camera_model.cy()) * s / camera_model.fy();
fake_p.z = s;
}
}
local_matrix_->undistort(*fake_cloud);
//global_matrix_->undistort(*fake_cloud);
std::stringstream fss;
fss << "/tmp/fake_cloud_" << static_cast<int>(s * 10) << ".txt";
std::ofstream fs(fss.str().c_str());
for (size_t j = 0; j < fake_cloud->height; ++j)
{
for (size_t i = 0; i < fake_cloud->width - 1; ++i)
{
fs << fake_cloud->at(i, j).z << ", ";
}
fs << fake_cloud->at(fake_cloud->width - 1, j).z << ";" << std::endl;
}
fs.close();
std::stringstream ss;
ss << "cloud_" << s;
viz1.addPointCloud(fake_cloud, color_handler, ss.str());
ss.str("");
ss << s << " m";
PCLPoint3 p = fake_cloud->points[280];
p.y -= 0.05f;
viz1.addText3D(ss.str(), p, 0.1, 1.0, 1.0, 1.0, ss.str());
viz1.spinOnce(10, true);
rate.sleep();
}
int ORIGINAL = 0, UNDISTORTED = 1, FINAL = 2;
pcl::visualization::PCLVisualizer viz("Test Set Visualization");
viz.createViewPort(0.0, 0.0, 0.33, 1.0, ORIGINAL);
viz.createViewPort(0.33, 0.0, 0.67, 1.0, UNDISTORTED);
viz.createViewPort(0.67, 0.0, 1.0, 1.0, FINAL);
// viz.createViewPort(0.0, 0.0, 0.5, 1.0, ORIGINAL);
// viz.createViewPort(0.5, 0.0, 1.0, 1.0, FINAL);
viz.addCoordinateSystem(0.5);
// std::vector<double> plane_distances(12);
// plane_distances[0] = 1.00134;
// plane_distances[1] = 1.20495;
// plane_distances[2] = 1.53675;
// plane_distances[3] = 1.86759;
// plane_distances[4] = 2.21074;
// plane_distances[5] = 2.54476;
// plane_distances[6] = 2.89025;
// plane_distances[7] = 3.23017;
// plane_distances[8] = 3.56103;
// plane_distances[9] = 3.89967;
// plane_distances[10] = 4.21813;
// plane_distances[11] = 4.57653;
std::vector<double> plane_distances;
// plane_distances.push_back(0.943);
// plane_distances.push_back(1.099);
// plane_distances.push_back(1.241);
// plane_distances.push_back(1.413);
// plane_distances.push_back(1.615);
// plane_distances.push_back(1.768);
// plane_distances.push_back(1.935);
// plane_distances.push_back(2.097);
// plane_distances.push_back(2.261);
// plane_distances.push_back(2.423);
// plane_distances.push_back(2.595);
// plane_distances.push_back(2.76);
// plane_distances.push_back(2.906);
// plane_distances.push_back(3.07);
// plane_distances.push_back(3.292);
// plane_distances.push_back(3.452);
// plane_distances.push_back(3.612);
// plane_distances.push_back(3.782);
// plane_distances.push_back(4.079);
// plane_distances.push_back(4.278);
// plane_distances.push_back(4.455);
// plane_distances.push_back(4.635);
plane_distances.push_back(0.962);
plane_distances.push_back(1.133);
plane_distances.push_back(1.305);
plane_distances.push_back(1.476);
plane_distances.push_back(1.666);
plane_distances.push_back(1.808);
plane_distances.push_back(1.998);
plane_distances.push_back(2.148);
plane_distances.push_back(2.339);
plane_distances.push_back(2.467);
plane_distances.push_back(2.619);
plane_distances.push_back(2.819);
plane_distances.push_back(3.013);
plane_distances.push_back(3.178);
plane_distances.push_back(3.323);
plane_distances.push_back(3.497);
plane_distances.push_back(3.672);
plane_distances.push_back(3.855);
plane_distances.push_back(4.02);
plane_distances.push_back(4.175);
plane_distances.push_back(4.338);
plane_distances.push_back(4.508);
size_t indices[] = {0, 6, 12, 18};
// plane_distances.push_back(0.927);
// plane_distances.push_back(1.106);
// plane_distances.push_back(1.331);
// plane_distances.push_back(1.507);
// plane_distances.push_back(1.661);
// plane_distances.push_back(1.855);
// plane_distances.push_back(2.012);
// plane_distances.push_back(2.258);
// plane_distances.push_back(2.457);
// plane_distances.push_back(2.675);
// plane_distances.push_back(2.817);
// plane_distances.push_back(3.015);
// plane_distances.push_back(3.179);
// plane_distances.push_back(5);
// plane_distances.push_back(3.531);
// plane_distances.push_back(3.705);
// plane_distances.push_back(3.862);
// plane_distances.push_back(3.99);
// plane_distances.push_back(5);
// plane_distances.push_back(4.411);
// size_t indices[] = {0, 6, 11, 17};
for (size_t index_i = 0; index_i < 4; ++index_i)
{
size_t index = indices[index_i];
std::stringstream ss;
ss << "cloud_" << index;
const RGBDData::ConstPtr & data = data_vec_[index];
const RGBDData::ConstPtr & part_und_data = part_und_data_vec_[index];
const RGBDData::ConstPtr & und_data = und_data_vec_[index];
// data->fuseData();
// part_und_data->fuseData();
// und_data->fuseData();
// viz.addPointCloud(data->fusedData(), ss.str(), ORIGINAL);
// viz.addPointCloud(part_und_data->fusedData(), ss.str() + "_und", UNDISTORTED);
// viz.addPointCloud(und_data->fusedData(), ss.str() + "_final", FINAL);
// pcl::ModelCoefficients coeffs;
// coeffs.values.resize(4);
// coeffs.values[0] = 0.00582574;
// coeffs.values[1] = 0.0834788;
// coeffs.values[2] = 0.996493;
// coeffs.values[3] = -plane_distances[index];
// ss.str("");
// ss << "plane_" << index;
// viz.addPlane(coeffs, ss.str());
// viz.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.1, 0.1, ss.str());
// viz.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, ss.str());
typedef pcl::visualization::PointCloudColorHandlerGenericField<PCLPoint3> ColorHandler;
viz.addPointCloud(data->depthData(), ColorHandler(data->depthData(), "y"), ss.str(), ORIGINAL);
viz.addPointCloud(part_und_data->depthData(), ColorHandler(part_und_data->depthData(), "y"), ss.str() + "_und", UNDISTORTED);
viz.addPointCloud(und_data->depthData(), ColorHandler(und_data->depthData(), "y"), ss.str() + "_final", FINAL);
ss.str("");
ss << index;
PCLPoint3 p1, p2;
// p1.x = -5; p1.y = 0; p1.z = plane_distances[index];
// p2.x = 5; p2.y = 0; p2.z = plane_distances[index];
p1.y = -5; p1.x = 0; p1.z = plane_distances[index];
p2.y = 5; p2.x = 0; p2.z = plane_distances[index];
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_o", ORIGINAL);
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_u", UNDISTORTED);
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_f", FINAL);
}
// viz.setCameraPosition(0.11384, -13.6088, 2.64823, 0.118126, 0.312233, 2.76886, 0.0, 0.0, 1.0);
// viz.setCameraClipDistances(9.85203, 18.1708);
viz.setCameraPosition(24.3543, 0.377151, 2.79902, 0.118126, 0.312233, 2.76886, 0.0, 1.0, 0.0);
viz.setCameraClipDistances(19.2645, 30.751);
viz.setCameraFieldOfView(30 * M_PI / 180);
viz.setPosition(0, 24);
viz.setSize(1920, 1056);
PCLPoint3 p1, p2;
// p1.x = -5; p1.y = 0; p1.z = 5;
// p2.x = 5; p2.y = 0; p2.z = 5;
p1.y = -5; p1.x = 0; p1.z = 5;
p2.y = 5; p2.x = 0; p2.z = 5;
viz.addLine(p1, p2, 0, 0, 0, "5_o", ORIGINAL);
viz.addLine(p1, p2, 0, 0, 0, "5_u", UNDISTORTED);
viz.addLine(p1, p2, 0, 0, 0, "5_f", FINAL);
/*for (int j = 1; j <= 5; j += 2)
{
std::stringstream ss;
ss << j << "m";
PCLPoint3 p1, p2;
p1.x = -5; p1.y = 0; p1.z = j;
p2.x = 5; p2.y = 0; p2.z = j;
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_o", ORIGINAL);
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_u", UNDISTORTED);
viz.addLine(p1, p2, 0, 0, 0, ss.str() + "_f", FINAL);
}*/
// viz.setBackgroundColor(0.08, 0.08, 0.08, UNDISTORTED);
viz.setBackgroundColor(1.0, 1.0, 1.0, ORIGINAL);
viz.setBackgroundColor(1.0, 1.0, 1.0, UNDISTORTED);
viz.setBackgroundColor(1.0, 1.0, 1.0, FINAL);
std::stringstream ss;
ss << "/home/filippo/Desktop/test/"
<< color_sensor_->cameraModel()->tfFrame() << "_"
<< MathTraits<LocalPolynomial>::MinDegree << "-" << MathTraits<LocalPolynomial>::Degree << "_"
<< local_matrix_->model()->binSize().x() << "x" << local_matrix_->model()->binSize().y() << ".pcd";
pcl::PCDWriter writer;
writer.write(ss.str(), *und_data_vec_[9]->depthData());
// viz.addText("TEST SET - VISUALIZATION", 20, 1010, 30, 1.0, 1.0, 1.0, "test_set_text", ORIGINAL);
// viz.addText("ORIGINAL CLOUDS", 20, 20, 30, 1.0, 1.0, 1.0, "original_text", ORIGINAL);
// viz.addText("UNDISTORTED CLOUDS", 20, 20, 30, 1.0, 1.0, 1.0, "undistorted_text", UNDISTORTED);
// viz.addText("FINAL CLOUDS", 20, 20, 30, 1.0, 1.0, 1.0, "final_text", FINAL);
/*ros::Rate rate = ros::Rate(100.0);
for (int i = 0; i < 500; ++i)
{
viz.spinOnce(10);
rate.sleep();
}
for (size_t index = 1; index < data_vec_.size(); index += 3)
{
PCLCloud3::Ptr tmp_cloud = boost::make_shared<PCLCloud3>(*data_vec_[index]->depthData());
for (float s = 0.0f; s <= 1.0f; s += 0.02f)
{
for (size_t i = 0; i < tmp_cloud->size(); ++i)
{
PCLPoint3 & tmp_p = tmp_cloud->points[i];
const PCLPoint3 & p = data_vec_[index]->depthData()->points[i];
const PCLPoint3 & und_p = und_data_vec_[index]->depthData()->points[i];
tmp_p.x = und_p.x * s + p.x * (1.0f - s);
tmp_p.y = und_p.y * s + p.y * (1.0f - s);
tmp_p.z = und_p.z * s + p.z * (1.0f - s);
}
RGBDData::Ptr tmp_data(boost::make_shared<RGBDData>(index));
tmp_data->setColorSensor(color_sensor_);
tmp_data->setDepthSensor(depth_sensor_);
tmp_data->setColorData(data_vec_[index]->colorData());
tmp_data->setDepthData(*tmp_cloud);
std::stringstream ss;
ss << "Cloud_" << index << "_und";
tmp_data->fuseData();
viz.updatePointCloud(tmp_data->fusedData(), ss.str());
viz.spinOnce(10, true);
rate.sleep();
}
}*/
/*PCLCloud3::Ptr tmp_cloud = boost::make_shared<PCLCloud3>(*cloud);
for (float s = 0.0f; s <= 1.0f; s += 0.02f)
{
for (size_t i = 0; i < tmp_cloud->size(); ++i)
{
PCLPoint3 & tmp_p = tmp_cloud->points[i];
const PCLPoint3 & p = cloud->points[i];
const PCLPoint3 & und_p = und_cloud->points[i];
tmp_p.x = und_p.x * s + p.x * (1.0f - s);
tmp_p.y = und_p.y * s + p.y * (1.0f - s);
tmp_p.z = und_p.z * s + p.z * (1.0f - s);
}
RGBDData::Ptr tmp_data(boost::make_shared<RGBDData>(index));
tmp_data->setColorSensor(color_sensor_);
tmp_data->setDepthSensor(depth_sensor_);
tmp_data->setColorData(rectified);
tmp_data->setDepthData(*tmp_cloud);
tmp_data->fuseData();
viz.updatePointCloud(tmp_data->fusedData(), "Cloud");
viz.spinOnce(10, true);
rate.sleep();
}*/
viz.spin();
}
void CalibrationTest::testPlanarityError() const
{
PolynomialUndistortionMatrixIO<LocalPolynomial> io;
io.write(*local_matrix_->model(), "/tmp/local_matrix.txt");
int index = 0;
for (Scalar i = 1.0; i < 5.5; i += 0.125, ++index)
{
Scalar max;
std::stringstream ss;
ss << "/tmp/matrix_"<< index << ".png";
io.writeImageAuto(*local_matrix_->model(), i, ss.str(), max);
ROS_INFO_STREAM("Max " << i << ": " << max);
}
std::vector<CheckerboardViews::Ptr> cb_views_vec;
CheckerboardViewsExtraction cb_extractor;
cb_extractor.setColorSensorPose(color_sensor_->pose());
cb_extractor.setCheckerboardVector(cb_vec_);
cb_extractor.setInputData(und_data_vec_);
cb_extractor.extractAll(cb_views_vec);
std::map<Scalar, std::vector<Scalar> > data_map;
for (size_t i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
RGBDData::ConstPtr und_data = cb_views.data();
RGBDData::ConstPtr data = data_map_.at(und_data);
const Cloud3 & und_points = PCLConversion<Scalar>::toPointMatrix(*und_data->depthData(), *cb_views.planeInliers());
Plane und_plane = PlaneFit<Scalar>::fit(und_points);
Scalar d_mean = 0;
Scalar und_mean = 0;
Scalar und_max = 0;
Scalar und_var = 0;
int count = 0;
for (int p = 0; p < und_points.elements(); ++p)
{
if (not und_points[p].allFinite())
continue;
d_mean += und_points[p].z();
Scalar d = und_plane.absDistance(und_points[p]);
und_mean += d;
und_var += d * d;
if (d > und_max)
und_max = d;
++count;
}
d_mean /= count;
und_mean /= count;
und_var /= count;
//und_var -= und_mean * und_mean;
const Cloud3 points = PCLConversion<Scalar>::toPointMatrix(*data->depthData(), *cb_views.planeInliers());
Plane plane = PlaneFit<Scalar>::fit(points);
Scalar mean = 0;
Scalar max = 0;
Scalar var = 0;
for (int p = 0; p < points.elements(); ++p)
{
if (not points[p].allFinite())
continue;
Scalar d = plane.absDistance(points[p]);
mean += d;
var += d * d;
if (d > max)
max = d;
}
mean /= count;
var /= count;
//var -= mean * mean;
data_map[d_mean].push_back(mean);
data_map[d_mean].push_back(std::sqrt(var));
data_map[d_mean].push_back(max);
data_map[d_mean].push_back(und_mean);
data_map[d_mean].push_back(std::sqrt(und_var));
data_map[d_mean].push_back(und_max);
data_map[d_mean].push_back(count);
}
// std::stringstream ss;
// ss << "/home/filippo/Desktop/test/"
// << color_sensor_->cameraModel()->tfFrame() << "_"
// << MathTraits<LocalPolynomial>::MinDegree << "-" << MathTraits<LocalPolynomial>::Degree << "_"
// << local_matrix_->model()->binSize().x() << "x" << local_matrix_->model()->binSize().y() << ".txt";
// std::ofstream fs(ss.str().c_str());
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;
for (std::map<Scalar, std::vector<Scalar> >::const_iterator it = data_map.begin(); it != data_map.end(); ++it)
std::cout << it->first << " "
<< it->second[0] << " " << it->second[1] << " " << it->second[2] << " "
<< it->second[3] << " " << it->second[4] << " " << it->second[5] << " "
<< static_cast<int>(it->second[6]) << std::endl;
// fs.close();
}
class NormalError
{
public:
NormalError(const PinholeCameraModel::ConstPtr & camera_model,
const Checkerboard::ConstPtr & checkerboard,
const Cloud2 & image_corners)
: camera_model_(camera_model),
checkerboard_(checkerboard),
image_corners_(image_corners)
{
}
template <typename T>
bool operator ()(const T * const rotation,
const T * const checkerboard_pose,
T * residuals) const
{
typename Types<T>::Pose checkerboard_pose_eigen = Types<T>::Pose::Identity();
checkerboard_pose_eigen.linear().template topLeftCorner<2, 2>() = Eigen::Rotation2D<T>(checkerboard_pose[0]).matrix();
checkerboard_pose_eigen.translation() = Eigen::Map<const Eigen::Matrix<T, 3, 1> >(&checkerboard_pose[1]);
Eigen::Matrix<T, 3, 3> new_base;
T unit_xyz[] = {0.0, 0.0, 1.0, 0.0, 0.0};
ceres::QuaternionRotatePoint(rotation, &unit_xyz[2], new_base.col(0).data());
ceres::QuaternionRotatePoint(rotation, &unit_xyz[1], new_base.col(1).data());
ceres::QuaternionRotatePoint(rotation, &unit_xyz[0], new_base.col(2).data());
checkerboard_pose_eigen = new_base * checkerboard_pose_eigen;
typename Types<T>::Cloud3 cb_corners(checkerboard_->corners().size());
cb_corners.container() = checkerboard_pose_eigen * checkerboard_->corners().container().cast<T>();
typename Types<T>::Cloud2 reprojected_corners = camera_model_->project3dToPixel<T>(cb_corners);
for (Size1 i = 0; i < cb_corners.elements(); ++i)
{
typename Types<T>::Vector2 diff = (reprojected_corners[i] - image_corners_[i].cast<T>());
residuals[2 * i + 0] = diff.x();
residuals[2 * i + 1] = diff.y();
}
return true;
}
private:
const PinholeCameraModel::ConstPtr & camera_model_;
const Checkerboard::ConstPtr & checkerboard_;
const Cloud2 & image_corners_;
};
void CalibrationTest::testCheckerboardError() const
{
std::vector<CheckerboardViews::Ptr> cb_views_vec;
CheckerboardViewsExtraction cb_extractor;
cb_extractor.setColorSensorPose(color_sensor_->pose());
cb_extractor.setCheckerboardVector(cb_vec_);
cb_extractor.setInputData(und_data_vec_);
cb_extractor.extractAll(cb_views_vec);
ceres::Problem problem;
Eigen::Matrix<Scalar, Eigen::Dynamic, 4, Eigen::DontAlign | Eigen::RowMajor> data(cb_views_vec.size(), 4);
Eigen::Matrix3d rot_matrix;
rot_matrix.col(2) = cb_views_vec[0]->colorCheckerboard()->plane().normal();
rot_matrix.col(0) = (Vector3::UnitX() - Vector3::UnitX().dot(rot_matrix.col(2)) * rot_matrix.col(2)).normalized();
rot_matrix.col(1) = rot_matrix.col(2).cross(rot_matrix.col(0));
Quaternion q(rot_matrix);
Eigen::Matrix<Scalar, 1, 4, Eigen::RowMajor | Eigen::DontAlign> rotation(q.w(), q.x(), q.y(), q.z());
for (Size1 i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
data.row(i)[0] = std::acos((cb_views.colorCheckerboard()->corners()(1, 0) - cb_views.colorCheckerboard()->corners()(0, 0)).normalized().x());
data.row(i).tail<3>() = rot_matrix.inverse() * cb_views.colorCheckerboard()->pose().translation();
NormalError * error = new NormalError(color_sensor_->cameraModel(),
cb_views.checkerboard(),
cb_views.colorView()->points());
typedef ceres::NumericDiffCostFunction<NormalError, ceres::CENTRAL, ceres::DYNAMIC, 4, 4> NormalCostFunction;
ceres::CostFunction * cost_function = new NormalCostFunction(error,
ceres::DO_NOT_TAKE_OWNERSHIP,
0 + 2 * cb_views.checkerboard()->size());
problem.AddResidualBlock(cost_function, NULL, rotation.data(), data.row(i).data());
}
problem.SetParameterization(rotation.data(), new ceres::QuaternionParameterization());
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 50;
options.minimizer_progress_to_stdout = true;
options.num_threads = 8;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::vector<Plane> plane_vec;
for (size_t i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
Pose checkerboard_pose_eigen = Pose::Identity();
checkerboard_pose_eigen.linear().template topLeftCorner<2, 2>() = Eigen::Rotation2Dd(data.row(i)[0]).matrix();
checkerboard_pose_eigen.translation() = data.row(i).tail<3>();
Quaternion q(rotation[0], rotation[1], rotation[2], rotation[3]);
Transform rot_matrix = q.matrix() * Transform::Identity();
Checkerboard cb(*cb_views.checkerboard());
cb.transform(rot_matrix * checkerboard_pose_eigen);
std::cout << cb.plane().normal().dot(cb.corners().container().rowwise().mean()) << std::endl;
cb.transform(color_sensor_->pose());
//std::cout << cb.plane().normal().transpose() << ": " << cb.plane().normal().dot(cb.corners().container().rowwise().mean()) << std::endl;
// AngleAxis rotation;
// rotation.angle() = data.row(i).head<3>().norm();
// rotation.axis() = data.row(i).head<3>().normalized();
// Translation3 translation(data.row(i).tail<3>());
// cb.transform(translation * rotation);
plane_vec.push_back(cb.plane());
}
Vector3 n_z = plane_vec[0].normal();
Vector3 n_x = (Vector3::UnitX() - Vector3::UnitX().dot(n_z) * n_z).normalized();
Vector3 n_y = n_z.cross(n_x);
for (size_t i = 0; i < cb_views_vec.size(); ++i)
{
const CheckerboardViews & cb_views = *cb_views_vec[i];
RGBDData::ConstPtr und_data = cb_views.data();
RGBDData::ConstPtr part_und_data = part_data_map_.at(und_data);
RGBDData::ConstPtr data = data_map_.at(und_data);
Plane plane = plane_vec[i]; //cb_views.colorCheckerboard()->plane();
//plane.transform(color_sensor_->pose());
const Cloud3 & und_points = PCLConversion<Scalar>::toPointMatrix(*und_data->depthData(), *cb_views.planeInliers());
PCLCloud3::Ptr tmp_und_cloud = boost::make_shared<PCLCloud3>(und_points.size().x(), und_points.size().y());
// PointPlaneExtraction<PCLPoint3> plane_extractor;
// plane_extractor.setInputCloud(und_data->depthData());
// const pcl::PointCloud<pcl::Normal>::ConstPtr und_normals_pcl = plane_extractor.cloudNormals();
// Cloud3 und_normals(Size2(cb_views.planeInliers()->size(), 1));
// for (Size1 j = 0; j < cb_views.planeInliers()->size(); ++j)
// {
// const pcl::Normal & p = und_normals_pcl->points[(*cb_views.planeInliers())[j]];
// und_normals[j] << p.normal_x, p.normal_y, p.normal_z;
// }
Point3 und_d_mean(0.0, 0.0, 0.0);
Scalar und_mean = 0;
Scalar und_mean_abs = 0;
Scalar und_mean2 = 0;
// Vector3 und_angle_mean = Vector3::Zero();
// Vector3 und_angle_mean2 = Vector3::Zero();
int count = 0, angle_count = 0;
for (int p = 0; p < und_points.elements(); ++p)
{
if (not und_points[p].allFinite())
continue;
und_d_mean += und_points[p];
Scalar d = plane.signedDistance(und_points[p]);
und_mean += d;
und_mean_abs += std::abs(d);
und_mean2 += d * d;
++count;
tmp_und_cloud->points[p].x = und_points[p].x();
tmp_und_cloud->points[p].y = und_points[p].y();
tmp_und_cloud->points[p].z = d;
// if (not und_normals[p].allFinite())
// continue;
// Scalar a = std::acos(und_normals[p].normalized().dot(n_z));
// if (a > M_PI_2)
// a = M_PI - a;
// und_angle_mean[2] += a;
// und_angle_mean2[2] += a * a;
// a = std::acos(und_normals[p].normalized().dot(n_x));
// und_angle_mean[0] += a;
// und_angle_mean2[0] += a * a;
// a = std::acos(und_normals[p].normalized().dot(n_y));
// und_angle_mean[1] += a;
// und_angle_mean2[1] += a * a;
// ++angle_count;
}
und_d_mean /= count;
und_mean /= count;
und_mean_abs /= count;
und_mean2 /= count;
// und_angle_mean /= angle_count;
// und_angle_mean2 /= angle_count;
Scalar und_std_dev = std::sqrt(und_mean2 - und_mean * und_mean);
// Vector3 und_angle_std_dev = (und_angle_mean2 - und_angle_mean.cwiseAbs2()).cwiseSqrt();
const Cloud3 points = PCLConversion<Scalar>::toPointMatrix(*data->depthData(), *cb_views.planeInliers());
PCLCloud3::Ptr tmp_cloud = boost::make_shared<PCLCloud3>(points.size().x(), points.size().y());
// plane_extractor.setInputCloud(data->depthData());
// const pcl::PointCloud<pcl::Normal>::ConstPtr normals_pcl = plane_extractor.cloudNormals();
// Cloud3 normals(Size2(cb_views.planeInliers()->size(), 1));
// for (Size1 j = 0; j < cb_views.planeInliers()->size(); ++j)
// {
// const pcl::Normal & p = normals_pcl->points[(*cb_views.planeInliers())[j]];
// normals[j] << p.normal_x, p.normal_y, p.normal_z;
// }
Point3 d_mean(0.0, 0.0, 0.0);
Scalar mean = 0;
Scalar mean2 = 0;
// Vector3 angle_mean = Vector3::Zero();
// Vector3 angle_mean2 = Vector3::Zero();
// angle_count = 0;
for (Size1 p = 0; p < points.elements(); ++p)
{
if (not points[p].allFinite())
continue;
d_mean += points[p];
Scalar d = plane.signedDistance(points[p]);
mean += d;
mean2 += d * d;
tmp_cloud->points[p].x = points[p].x();
tmp_cloud->points[p].y = points[p].y();
tmp_cloud->points[p].z = d;
// if (not und_normals[p].allFinite())
// continue;
// Scalar a = std::acos(normals[p].normalized().dot(n_z));
// if (a > M_PI_2)
// a = M_PI - a;
// angle_mean[2] += a;
// angle_mean2[2] += a * a;
// a = std::acos(normals[p].normalized().dot(n_x));
// angle_mean[0] += a;
// angle_mean2[0] += a * a;
// a = std::acos(normals[p].normalized().dot(n_y));
// angle_mean[1] += a;
// angle_mean2[1] += a * a;
// ++angle_count;
}
d_mean /= count;
mean /= count;
mean2 /= count;
// angle_mean /= angle_count;
// angle_mean2 /= angle_count;
Scalar std_dev = std::sqrt(mean2 - mean * mean);
// Vector3 angle_std_dev = (angle_mean2 - angle_mean.cwiseAbs2()).cwiseSqrt();
const Cloud3 part_points = PCLConversion<Scalar>::toPointMatrix(*part_und_data->depthData(), *cb_views.planeInliers());
Point3 part_d_mean(0.0, 0.0, 0.0);
Scalar part_mean = 0;
Scalar part_mean2 = 0;
for (int p = 0; p < part_points.elements(); ++p)
{
if (not part_points[p].allFinite())
continue;
part_d_mean += part_points[p];
Scalar d = plane.signedDistance(part_points[p]);
part_mean += d;
part_mean2 += d * d;
}
part_d_mean /= count;
part_mean /= count;
part_mean2 /= count;
Scalar part_std_dev = std::sqrt(part_mean2 - part_mean * part_mean);
Plane und_fitted_plane = PlaneFit<Scalar>::fit(und_points);
if (und_fitted_plane.offset() < 0)
und_fitted_plane.coeffs() *= -1.0;
Scalar und_angle_x = std::acos(und_fitted_plane.normal().dot(n_x));
Scalar und_angle_y = std::acos(und_fitted_plane.normal().dot(n_y));
Plane fitted_plane = PlaneFit<Scalar>::fit(points);
if (fitted_plane.offset() < 0)
fitted_plane.coeffs() *= -1.0;
Scalar angle_x = std::acos(fitted_plane.normal().dot(n_x));
Scalar angle_y = std::acos(fitted_plane.normal().dot(n_y));
Plane part_fitted_plane = PlaneFit<Scalar>::fit(part_points);
if (part_fitted_plane.offset() < 0)
part_fitted_plane.coeffs() *= -1.0;
Scalar part_angle_x = std::acos(part_fitted_plane.normal().dot(n_x));
Scalar part_angle_y = std::acos(part_fitted_plane.normal().dot(n_y));
std::cout << "Original: " << plane.normal().dot(d_mean) << ", " << mean << ", " << std_dev << ", "
<< 180 * angle_x / M_PI << ", " << 180 * angle_y / M_PI << "; "
<< "Final: " << plane.normal().dot(und_d_mean) << ", "<< und_mean << ", " << und_std_dev << ", "
<< 180 * und_angle_x / M_PI << ", " << 180 * und_angle_y / M_PI << "; "
<< "Undistorted: " << plane.normal().dot(part_d_mean) << ", "<< part_mean << ", " << part_std_dev << ", "
<< 180 * part_angle_x / M_PI << ", " << 180 * part_angle_y / M_PI << "; " << std::endl;
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(tmp_cloud);
voxel.setLeafSize(0.04f, 10.0f, 0.03f);
voxel.filter(*tmp_cloud);
std::fstream fs;
std::stringstream ss;
ss << "/tmp/points_" << i << ".txt";
fs.open(ss.str().c_str(), std::fstream::out);
for (size_t j = 0; j < tmp_cloud->size(); ++j)
fs << tmp_cloud->points[j].x << " " << tmp_cloud->points[j].z << std::endl;
fs.close();
voxel.setInputCloud(tmp_und_cloud);
voxel.filter(*tmp_und_cloud);
ss.str("");
ss << "/tmp/und_points_" << i << ".txt";
fs.open(ss.str().c_str(), std::fstream::out);
for (size_t j = 0; j < tmp_und_cloud->size(); ++j)
fs << tmp_und_cloud->points[j].x << " " << tmp_und_cloud->points[j].z << std::endl;
fs.close();
}
}
boost::shared_ptr<std::vector<int> >
CalibrationTest::extractPlaneFromCloud (const PCLCloud3::Ptr & cloud,
const Cloud2 & depth_corners) const
{
float min_x = depth_corners[0].x();
float max_x = min_x;
float min_y = depth_corners[0].y();
float max_y = min_y;
for (int i = 1; i < 4; ++i)
{
min_x = std::min<float>(min_x, depth_corners[i].x());
max_x = std::max<float>(max_x, depth_corners[i].x());
min_y = std::min<float>(min_y, depth_corners[i].y());
max_y = std::max<float>(max_y, depth_corners[i].y());
}
min_x = std::max(min_x, 0.0f);
max_x = std::min(max_x, static_cast<float>(cloud->width));
min_y = std::max(min_y, 0.0f);
max_y = std::min(max_y, static_cast<float>(cloud->height));
Vector2 lines[4];
for (int i = 0; i < 4; ++i)
lines[i] = depth_corners[(i + 1) % 4] - depth_corners[i];
boost::shared_ptr<std::vector<int> > indices = boost::make_shared<std::vector<int> >();
indices->reserve(static_cast<size_t>((max_x - min_x) * (max_y - min_y)));
for (float y = std::ceil(min_y); y < max_y; ++y)
{
for (float x = std::ceil(min_x); x < max_x; ++x)
{
bool ok = true;
Vector2 vec = Vector2(x, y) - depth_corners[0];
int sign;
if (lines[0].x() * vec.y() - lines[0].y() * vec.x() >= 0)
sign = 1;
else
sign = -1;
for (int i = 1; i < 4; ++i)
{
Vector2 vec = Vector2(x, y) - depth_corners[i];
if (sign * (lines[i].x() * vec.y() - lines[i].y() * vec.x()) < 0)
{
ok = false;
break;
}
}
if (ok)
indices->push_back(static_cast<int>(y * cloud->width + x));
}
}
return indices;
}
void CalibrationTest::testCube() const
{
Checkerboard checkerboard(4, 4, 0.095, 0.095);
std::vector<std::vector<double> > data_vec;
for (size_t i = 0; i < und_data_vec_.size(); ++i)
{
#ifndef UNCALIBRATED
const RGBDData & data = *und_data_vec_.at(i);
#else
const RGBDData & data = *data_vec_.at(i);
#endif
cv::Mat tmp_image = data.colorData().clone();
//cv::Mat tmp_image_rect = tmp_image;
/*cv::undistort(tmp_image, tmp_image_rect,
color_sensor_->cameraModel()->intrinsicMatrix(),
color_sensor_->cameraModel()->distortionCoeffs(),
color_sensor_->cameraModel()->intrinsicMatrix());*/
//color_sensor_->cameraModel()->rectifyImage(tmp_image, tmp_image_rect);
AutomaticCheckerboardFinder finder;
std::vector<Plane, Eigen::aligned_allocator<Plane> > planes;
std::vector<Point3, Eigen::aligned_allocator<Point3> > points;
std::vector<Plane, Eigen::aligned_allocator<Plane> > depth_planes;
std::vector<Point3, Eigen::aligned_allocator<Point3> > depth_points;
//pcl::visualization::PCLVisualizer viz("VIZ");
for (int c = 0; c < 3; ++c)
{
finder.setImage(tmp_image);
Cloud2 corners;
bool found = finder.find(checkerboard, corners);
Pose pose = color_sensor_->estimatePose(corners, checkerboard.corners());
Checkerboard tmp_checkerboard(6, 6, 0.095, 0.095);
tmp_checkerboard.transform(pose * Translation3(-0.095, -0.095, 0.0));
Cloud2 tmp_corners = color_sensor_->cameraModel()->project3dToPixel2(tmp_checkerboard.corners());
if (found)
{
cv::vector<cv::Point> points;
/*points.push_back(cv::Point(corners[0].x(), corners[0].y()));
points.push_back(cv::Point(corners[3].x(), corners[3].y()));
points.push_back(cv::Point(corners[15].x(), corners[15].y()));
points.push_back(cv::Point(corners[12].x(), corners[12].y()));*/
points.push_back(cv::Point(tmp_corners[0].x(), tmp_corners[0].y()));
points.push_back(cv::Point(tmp_corners[5].x(), tmp_corners[5].y()));
points.push_back(cv::Point(tmp_corners[35].x(), tmp_corners[35].y()));
points.push_back(cv::Point(tmp_corners[30].x(), tmp_corners[30].y()));
cv::fillConvexPoly(tmp_image, points, cv::Scalar(c == 0 ? 128 : 0, c == 1 ? 128 : 0, c == 2 ? 128 : 0));
for (int corner = 0; corner < tmp_corners.elements(); ++corner)
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));
//cv::fillConvexPoly(tmp_image_rect, points, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));
}
else
{
ROS_WARN("Pattern not found");
InteractiveCheckerboardFinder i_finder;
i_finder.setImage(tmp_image);
found = i_finder.find(checkerboard, corners, true);
Pose pose = color_sensor_->estimatePose(corners, checkerboard.corners());
tmp_checkerboard = Checkerboard(6, 6, 0.095, 0.095);
tmp_checkerboard.transform(pose * Translation3(-0.095, -0.095, 0.0));
Cloud2 tmp_corners = color_sensor_->cameraModel()->project3dToPixel2(tmp_checkerboard.corners());
if (found)
{
cv::vector<cv::Point> points;
points.push_back(cv::Point(tmp_corners[0].x(), tmp_corners[0].y()));
points.push_back(cv::Point(tmp_corners[5].x(), tmp_corners[5].y()));
points.push_back(cv::Point(tmp_corners[35].x(), tmp_corners[35].y()));
points.push_back(cv::Point(tmp_corners[30].x(), tmp_corners[30].y()));
cv::fillConvexPoly(tmp_image, points, cv::Scalar(c == 0 ? 128 : 0, c == 1 ? 128 : 0, c == 2 ? 128 : 0));
for (int corner = 0; corner < tmp_corners.elements(); ++corner)
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));
//cv::fillConvexPoly(tmp_image_rect, points, cv::Scalar(c == 0 ? 255 : 0, c == 1 ? 255 : 0, c == 2 ? 255 : 0));
}
}
//cv::drawChessboardCorners(tmp_image, cv::Size(checkerboard.cols(), checkerboard.rows()), corners, found);
Cloud3 depth_corners(Size2(1, 4));
depth_corners(0, 0) = tmp_checkerboard(0, 0);
depth_corners(0, 1) = tmp_checkerboard(0, 5);
depth_corners(0, 2) = tmp_checkerboard(5, 5);
depth_corners(0, 3) = tmp_checkerboard(5, 0);
depth_corners.transform(color_sensor_->pose());
//#ifdef HERRERA
std::vector<cv::Point3f> in(4);
in[0] = cv::Point3f(depth_corners(0, 0).x(), depth_corners(0, 0).y(), depth_corners(0, 0).z());
in[1] = cv::Point3f(depth_corners(0, 1).x(), depth_corners(0, 1).y(), depth_corners(0, 1).z());
in[2] = cv::Point3f(depth_corners(0, 2).x(), depth_corners(0, 2).y(), depth_corners(0, 2).z());
in[3] = cv::Point3f(depth_corners(0, 3).x(), depth_corners(0, 3).y(), depth_corners(0, 3).z());
std::vector<cv::Point2f> out;
//#ifdef HERRERA
cv::projectPoints(in, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
depth_sensor_->cameraModel()->intrinsicMatrix(), depth_sensor_->cameraModel()->distortionCoeffs(), out);
//#else
// cv::projectPoints(in, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
// depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);
//#endif
Cloud2 image_corners(Size2(1, 4));
image_corners(0, 0) = Point2(out[0].x, out[0].y);
image_corners(0, 1) = Point2(out[1].x, out[1].y);
image_corners(0, 2) = Point2(out[2].x, out[2].y);
image_corners(0, 3) = Point2(out[3].x, out[3].y);
//#else
// Cloud2 image_corners = depth_sensor_->cameraModel()->project3dToPixel(depth_corners);
//#endif
boost::shared_ptr<std::vector<int> > indices = extractPlaneFromCloud(data.depthData(), image_corners);
data.fuseData();
boost::shared_ptr<std::vector<int> > new_indices = boost::shared_ptr<std::vector<int> >(new std::vector<int>());
new_indices->reserve(indices->size());
for (size_t i_i = 0; i_i < indices->size(); ++i_i)
{
const pcl::PointXYZRGB & point = (*data.fusedData())[(*indices)[i_i]];
if (point.r + point.g + point.b > 300)
new_indices->push_back((*indices)[i_i]);
}
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(data.fusedData());
sor.setIndices(new_indices);
sor.setMeanK(10);
sor.setStddevMulThresh(1.0);
sor.filter(*new_indices);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(data.depthData());
extract.setIndices(new_indices);
extract.setNegative(false);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
extract.filter (*cloud_p);
/*std::stringstream ss;
ss << "cloud_" << c;
viz.addPointCloud(cloud_p, ss.str());*/
Cloud3 checkerboards_3d = PCLConversion<Scalar>::toPointMatrix(*data.depthData(), *new_indices);
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*data.depthData(), *new_indices, centroid);
Point3 point = color_sensor_->pose().inverse() * centroid.cast<Scalar>().head<3>();
checkerboards_3d.transform(color_sensor_->pose().inverse());
Cloud2 proj = color_sensor_->cameraModel()->project3dToPixel2(checkerboards_3d);
for (Size1 i = 0; i < proj.elements(); ++i)
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));
PlaneFit<Scalar> plane_fit;
Plane plane = plane_fit.fit(checkerboards_3d);
/*pcl::ModelCoefficients cf;
cf.values.resize(4);
cf.values[0] = plane.coeffs()[0];
cf.values[1] = plane.coeffs()[1];
cf.values[2] = plane.coeffs()[2];
cf.values[3] = plane.coeffs()[3];
viz.addPlane(cf, ss.str() + "_plane");
pcl::ModelCoefficients cf2;
cf2.values.resize(4);
cf2.values[0] = tmp_checkerboard.plane().coeffs()[0];
cf2.values[1] = tmp_checkerboard.plane().coeffs()[1];
cf2.values[2] = tmp_checkerboard.plane().coeffs()[2];
cf2.values[3] = tmp_checkerboard.plane().coeffs()[3];
viz.addPlane(cf2, ss.str() + "_cb");*/
point = plane.projection(point);
if (found)
{
planes.push_back(tmp_checkerboard.plane());
points.push_back(tmp_checkerboard.corners()[0]);
depth_planes.push_back(plane);
depth_points.push_back(point);
}
}
//viz.spin();
Point3 rgb_point, depth_point;
data_vec.push_back(std::vector<double>(20));
{
Eigen::Matrix<Scalar, 3, 3> N;
N.col(0) = planes.at(0).normal();
N.col(1) = planes.at(1).normal();
N.col(2) = planes.at(2).normal();
rgb_point = 1.0 / N.determinant() * (points.at(0).dot(N.col(0)) * N.col(1).cross(N.col(2)) +
points.at(1).dot(N.col(1)) * N.col(2).cross(N.col(0)) +
points.at(2).dot(N.col(2)) * N.col(0).cross(N.col(1)));
//Point2 pixel = color_sensor_->cameraModel()->project3dToPixel(rgb_point);
//cv::circle(tmp_image_rect, cv::Point(pixel.x(), pixel.y()), 3, cv::Scalar(0, 0, 0));
std::vector<cv::Point3f> points(1);
points[0] = cv::Point3f(rgb_point.x(), rgb_point.y(), rgb_point.z());
std::vector<cv::Point2f> out(1);
//cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
// depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);
//#ifdef HERRERA
cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(), out);
/*#else
cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
color_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);
#endif*/
cv::circle(tmp_image, out[0], 3, cv::Scalar(0, 0, 255));
Scalar a0 = 90.0 - 180.0 / M_PI * std::acos(N.col(0).dot(N.col(1)));
Scalar a1 = 90.0 - 180.0 / M_PI * std::acos(N.col(1).dot(N.col(2)));
Scalar a2 = 90.0 - 180.0 / M_PI * std::acos(N.col(2).dot(N.col(0)));
ROS_INFO_STREAM("Pixel RGB: " << out[0].x << ", " << out[0].y);
ROS_INFO_STREAM("Angular error RGB: " << a0 << ", " << a1 << ", " << a2);
data_vec.back().at(0) = out[0].x;
data_vec.back().at(1) = out[0].y;
data_vec.back().at(2) = a0;
data_vec.back().at(3) = a1;
data_vec.back().at(4) = a2;
}
{
Eigen::Matrix<Scalar, 3, 3> N;
N.col(0) = depth_planes.at(0).normal();
N.col(1) = depth_planes.at(1).normal();
N.col(2) = depth_planes.at(2).normal();
depth_point = 1.0 / N.determinant() * (depth_points.at(0).dot(N.col(0)) * N.col(1).cross(N.col(2)) +
depth_points.at(1).dot(N.col(1)) * N.col(2).cross(N.col(0)) +
depth_points.at(2).dot(N.col(2)) * N.col(0).cross(N.col(1)));
//Point2 pixel = color_sensor_->cameraModel()->project3dToPixel(depth_point);
//cv::circle(tmp_image_rect, cv::Point(pixel.x(), pixel.y()), 3, cv::Scalar(0, 0, 255));
std::vector<cv::Point3f> points(1);
points[0] = cv::Point3f(depth_point.x(), depth_point.y(), depth_point.z());
std::vector<cv::Point2f> out(1);
//cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
// depth_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);
//#ifdef HERRERA
cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(), out);
/*#else
cv::projectPoints(points, cv::Mat_<float>::zeros(3, 1), cv::Mat_<float>::zeros(3, 1),
color_sensor_->cameraModel()->intrinsicMatrix(), cv::Mat(), out);
#endif*/
cv::circle(tmp_image, out[0], 3, cv::Scalar(0, 0, 0));
Scalar a0 = 90.0 - 180.0 / M_PI * std::acos(N.col(0).dot(N.col(1)));
Scalar a1 = 90.0 - 180.0 / M_PI * std::acos(N.col(1).dot(N.col(2)));
Scalar a2 = 90.0 - 180.0 / M_PI * std::acos(N.col(2).dot(N.col(0)));
ROS_INFO_STREAM("Pixel DEPTH: " << out[0].x << ", " << out[0].y);
ROS_INFO_STREAM("Angular error DEPTH: " << a0 << ", " << a1 << ", " << a2);
data_vec.back().at(5) = out[0].x;
data_vec.back().at(6) = out[0].y;
data_vec.back().at(7) = a0;
data_vec.back().at(8) = a1;
data_vec.back().at(9) = a2;
}
Scalar a0 = 180.0 / M_PI * std::acos(planes.at(0).normal().dot(depth_planes.at(0).normal()));
Scalar a1 = 180.0 / M_PI * std::acos(planes.at(1).normal().dot(depth_planes.at(1).normal()));
Scalar a2 = 180.0 / M_PI * std::acos(planes.at(2).normal().dot(depth_planes.at(2).normal()));
ROS_INFO_STREAM("Point RGB: " << rgb_point.x() << ", " << rgb_point.y() << ", " << rgb_point.z());
ROS_INFO_STREAM("Point DEPTH: " << depth_point.x() << ", " << depth_point.y() << ", " << depth_point.z());
ROS_INFO_STREAM("Depth error: " << (rgb_point - depth_point).norm());
//ROS_INFO_STREAM(rgb_point.transpose() << " -- " << depth_point.transpose() << " => " << (rgb_point - depth_point).norm());
ROS_INFO_STREAM("Angular error: " << (a0 > 90 ? 180 - a0 : a0) << ", " << (a1 > 90 ? 180 - a1 : a1) << ", " << (a2 > 90 ? 180 - a2 : a2));
ROS_INFO_STREAM(planes.at(0).normal().transpose() << " -- " << planes.at(1).normal().transpose() << " -- " << planes.at(2).normal().transpose() << " -- ");
cv::imshow("Image", tmp_image);
cv::waitKey();
data_vec.back().at(10) = rgb_point.x();
data_vec.back().at(11) = rgb_point.y();
data_vec.back().at(12) = rgb_point.z();
data_vec.back().at(13) = depth_point.x();
data_vec.back().at(14) = depth_point.y();
data_vec.back().at(15) = depth_point.z();
data_vec.back().at(16) = (rgb_point - depth_point).norm();
data_vec.back().at(17) = (a0 > 90 ? 180 - a0 : a0);
data_vec.back().at(18) = (a1 > 90 ? 180 - a1 : a1);
data_vec.back().at(19) = (a2 > 90 ? 180 - a2 : a2);
}
for (size_t i = 0; i < data_vec.size(); ++i)
for (size_t j = 0; j < data_vec[i].size(); ++j)
std::cout << data_vec[i][j] << (j == data_vec[i].size() - 1 ? "\n" : ", ");
}
} /* namespace calibration */
================================================
FILE: src/rgbd_calibration/checkerboard_views.cpp
================================================
/*
* Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <calibration_common/base/pcl_conversion.h>
#include <calibration_common/ceres/plane_fit.h>
#include <rgbd_calibration/checkerboard_views.h>
namespace calibration
{
void CheckerboardViews::setColorView(const PinholeView<Checkerboard>::ConstPtr & color_view)
{
color_view_ = boost::make_shared<PinholeView<Checkerboard> >(*color_view);
// color_checkerboard_ = boost::make_shared<Checkerboard>(*color_view_);
std::stringstream ss;
ss << checkerboard_->frameId() << "_" << id_;
color_checkerboard_ = boost::make_shared<Checkerboard>(*checkerboard_);
color_checkerboard_->setParent(data_->colorSensor());
color_checkerboard_->setFrameId(ss.str());
color_checkerboard_->transform(color_view_->sensor()->estimatePose(color_view_->points(), checkerboard_->corners()));
}
void CheckerboardViews::setImageCorners(const Cloud2 & image_corners)
{
PinholeView<Checkerboard>::Ptr color_view = boost::make_shared<PinholeView<Checkerboard> >();
color_view->setId(id_);
color_view->setPoints(image_corners);
color_view->setSensor(data_->colorSensor());
color_view->setObject(checkerboard_);
setColorView(color_view);
}
void CheckerboardViews::setPlaneInliers(const pcl::IndicesConstPtr & plane_inliers,
Scalar inliers_std_dev)
{
plane_inliers_ = plane_inliers;
depth_view_ = boost::make_shared<DepthViewPCL<PlanarObject> >();
depth_view_->setId(id_);
depth_view_->setData(data_->depthData());
depth_view_->setPoints(*plane_inliers);
depth_view_->setSensor(data_->depthSensor());
depth_view_->setObject(checkerboard_);
depth_plane_ = boost::make_shared<PlanarObject>();
depth_plane_->setParent(data_->depthSensor());
std::stringstream ss;
ss << checkerboard_->frameId() << "_plane_" << id_;
depth_plane_->setFrameId(ss.str());
depth_plane_->setPlane(PlaneFit<Scalar>::robustFit(PCLConversion<Scalar>::toPointMatrix(*depth_view_->data(), depth_view_->points()), inliers_std_dev));
//depth_plane_->setPlane(PlaneFit<Scalar>::fit(depth_view_->points()));
}
void CheckerboardViews::setPlaneInliers(const PlaneInfo & plane_info)
{
plane_inliers_ = plane_info.indices_;
depth_view_ = boost::make_shared<DepthViewPCL<PlanarObject> >();
depth_view_->setId(id_);
depth_view_->setData(data_->depthData());
// depth_view_->setPoints(PCLConversion<Scalar>::toPointMatrix(*data_->depthData(), *plane_info.indices_));
depth_view_->setPoints(*plane_info.indices_);
depth_view_->setSensor(data_->depthSensor());
depth_view_->setObject(checkerboard_);
depth_plane_ = boost::make_shared<PlanarObject>();
depth_plane_->setParent(data_->depthSensor());
std::stringstream ss;
ss << checkerboard_->frameId() << "_plane_" << id_;
depth_plane_->setFrameId(ss.str());
depth_plane_->setPlane(plane_info.plane_);
}
void CheckerboardViews::draw(cv::Mat & image) const
{
cv::Size pattern_size(color_checkerboard_->rows(), color_checkerboard_->cols());
std::vector<cv::Point2f> corners;
for (int i = 0; i < color_view_->points().size().prod(); ++i)
corners.push_back(cv::Point2f(color_view_->points()[i][0], color_view_->points()[i][1]));
cv::drawChessboardCorners(image, pattern_size, cv::Mat(corners), true);
}
} /* namespace calibration */
================================================
FILE: src/rgbd_calibration/checkerboard_views_extractor.cpp
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rgbd_calibration/checkerboard_views_extractor.h>
#include <calibration_common/algorithms/plane_extraction.h>
#include <calibration_common/algorithms/interactive_checkerboard_finder.h>
#include <calibration_common/algorithms/automatic_checkerboard_finder.h>
namespace calibration
{
Size1 CheckerboardViewsExtraction::extract(const RGBDData::ConstPtr & data,
std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive,
bool force) const
{
Size1 added = 0;
cv::Mat image = data->colorData().clone();
AutomaticCheckerboardFinder finder;
finder.setImage(image);
PointPlaneExtraction<PCLPoint3>::Ptr plane_extractor;
if (not interactive)
plane_extractor = boost::make_shared<PointPlaneExtraction<PCLPoint3> >();
else
plane_extractor = boost::make_shared<PointPlaneExtractionGUI<PCLPoint3> >();
plane_extractor->setInputCloud(data->depthData());
for (Size1 c = 0; c < cb_vec_.size(); ++c)
{
const Checkerboard::ConstPtr & cb = cb_vec_[c];
plane_extractor->setRadius(std::min(cb->width(), cb->height()) / 1.5);
std::stringstream ss;
ss << "rgbd_cb_" << data->id() << "_" << c;
CheckerboardViews::Ptr cb_views(boost::make_shared<CheckerboardViews>(ss.str()));
cb_views->setData(data);
cb_views->setCheckerboard(cb);
// 1. Extract corners
Cloud2 image_corners(cb->corners().size());
if (not finder.find(*cb, image_corners))
{
if (force)
{
InteractiveCheckerboardFinder finder2;
finder2.setImage(image);
if (not finder2.find(*cb, image_corners))
continue;
}
else
continue;
}
cb_views->setImageCorners(image_corners);
if (not cb_constraint_->isValid(*cb_views->colorCheckerboard()))
continue;
// 2. Extract plane
if (not only_images_)
{
PlaneInfo plane_info;
if (interactive)
{
cb_views->draw(image);
}
else
{
Point3 center = color_sensor_pose_ * cb_views->colorCheckerboard()->center();
PCLPoint3 p;
p.x = center[0];
p.y = center[1];
p.z = center[2];
plane_extractor->setPoint(p);
}
if (not plane_extractor->extract(plane_info))
continue;
cb_views->setPlaneInliers(plane_info.indices_, plane_info.std_dev_);
if (not plane_constraint_->isValid(*cb_views->depthPlane()))
continue;
}
#pragma omp critical
cb_views_vec.push_back(cb_views);
++added;
}
return added;
}
Size1 CheckerboardViewsExtraction::extract(std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive) const
{
return extract(data_, cb_views_vec, interactive, true);
}
Size1 CheckerboardViewsExtraction::extractAll(std::vector<CheckerboardViews::Ptr> & cb_views_vec,
bool interactive) const
{
Size1 added = 0;
#pragma omp parallel for
for (Size1 i = 0; i < data_vec_.size(); ++i)
{
Size1 n = extract(data_vec_[i], cb_views_vec, interactive, force_);
#pragma omp atomic
added += n;
}
return added;
}
} /* namespace calibration */
================================================
FILE: src/rgbd_calibration/data_collection_node.cpp
================================================
#include <ros/ros.h>
#include <ros/topic.h>
#include <pcl/io/pcd_io.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
//#include <pcl/point_types.h>
//#include <pcl_ros/point_cloud.h>
#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <calibration_common/pinhole/pinhole.h>
#include <calibration_common/objects/checkerboard.h>
#include <calibration_common/algorithms/automatic_checkerboard_finder.h>
#include <camera_info_manager/camera_info_manager.h>
#include <calibration_msgs/CheckerboardArray.h>
#include <calibration_msgs/CheckerboardMsg.h>
#include <rgbd_calibration/Acquisition.h>
using namespace camera_info_manager;
using namespace calibration_msgs;
using rgbd_calibration::Acquisition;
namespace calibration
{
class DataCollectionNode
{
public:
enum
{
SAVE_NONE = 0,
SAVE_IMAGE = 1,
SAVE_DEPTH_IMAGE = 2,
SAVE_POINT_CLOUD = 4,
SAVE_IMAGE_CAMERA_INFO = 8,
SAVE_DEPTH_CAMERA_INFO = 16,
SAVE_ALL = 31
};
enum DepthType
{
DEPTH_FLOAT32,
DEPTH_UINT16
};
DataCollectionNode(ros::NodeHandle & node_handle);
virtual
~DataCollectionNode();
bool
initialize();
static Checkerboard::Ptr
createCheckerboard(const CheckerboardMsg::ConstPtr & msg,
int id);
static Checkerboard::Ptr
createCheckerboard(const CheckerboardMsg & msg,
int id);
protected:
void
save(const sensor_msgs::CameraInfo::ConstPtr & camera_info,
const std::string & file_name);
void
save(const pcl::PCLPointCloud2::ConstPtr & cloud,
const std::string & file_name);
void
save(const cv::Mat & image,
const std::string & file_name);
void
saveDepth(const cv::Mat & depth_image,
const std::string & file_name);
void
pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr & msg);
void
imageCallback(const sensor_msgs::Image::ConstPtr & msg);
void
depthImageCallback(const sensor_msgs::Image::ConstPtr & msg);
void
cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg);
void
depthCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg);
void
actionCallback(const Acquisition::ConstPtr & msg);
void
checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg);
bool
waitForMessages();
/* variables */
ros::NodeHandle node_handle_;
image_transport::ImageTransport image_transport_;
ros::Subscriber cloud_sub_;
image_transport::Subscriber image_sub_;
image_transport::Subscriber depth_image_sub_;
ros::Subscriber camera_info_sub_;
ros::Subscriber depth_camera_info_sub_;
ros::Subscriber omnicamera_info_sub_;
ros::Subscriber action_sub_;
pcl::PCLPointCloud2::Ptr cloud_msg_;
sensor_msgs::Image::ConstPtr image_msg_;
sensor_msgs::Image::ConstPtr depth_image_msg_;
sensor_msgs::CameraInfo::ConstPtr camera_info_msg_;
sensor_msgs::CameraInfo::ConstPtr depth_camera_info_msg_;
// TODO find another way to get checkerboards
ros::Subscriber checkerboards_sub_;
calibration_msgs::CheckerboardArray::ConstPtr checkerboard_array_msg_;
std::vector<Checkerboard::ConstPtr> cb_vec_;
std::vector<std::vector<cv::Point2f> > last_corners_;
std::vector<std::vector<cv::Point2f> > last_saved_corners_;
int starting_index_;
int file_index_;
std::string save_folder_;
std::string image_extension_;
std::string image_filename_;
std::string depth_filename_;
std::string cloud_filename_;
bool search_checkerboard_;
int save_flags_;
DepthType depth_type_;
};
DataCollectionNode::DataCollectionNode(ros::NodeHandle & node_handle)
: node_handle_(node_handle),
image_transport_(node_handle),
search_checkerboard_(false)
{
checkerboards_sub_ = node_handle_.subscribe("checkerboard_array", 1, &DataCollectionNode::checkerboardArrayCallback, this);
action_sub_ = node_handle.subscribe("action", 1, &DataCollectionNode::actionCallback, this);
node_handle_.param("starting_index", starting_index_, 1);
if (not node_handle_.getParam("save_folder", save_folder_))
ROS_FATAL_STREAM("[" << ros::this_node::getName() << "] Missing folder!!");
if (save_folder_.at(save_folder_.size() - 1) != '/')
save_folder_.append("/");
file_index_ = starting_index_;
node_handle_.param("image_extension", image_extension_, std::string("png"));
node_handle_.param("image_filename", image_filename_, std::string("image_"));
node_handle_.param("depth_filename", depth_filename_, std::string("depth_"));
node_handle_.param("cloud_filename", cloud_filename_, std::string("cloud_"));
node_handle_.param("search_checkerboard", search_checkerboard_, false);
bool save_image, save_image_camera_info;
node_handle_.param("save_image", save_image, false);
node_handle_.param("save_image_camera_info", save_image_camera_info, false);
bool save_depth_image, save_depth_camera_info;
node_handle_.param("save_depth_image", save_depth_image, false);
node_handle_.param("save_depth_camera_info", save_depth_camera_info, false);
bool save_point_cloud;
node_handle_.param("save_point_cloud", save_point_cloud, false);
save_flags_ = 0;
save_flags_ |= save_image ? SAVE_IMAGE : 0;
save_flags_ |= save_image_camera_info ? SAVE_IMAGE_CAMERA_INFO : 0;
save_flags_ |= save_depth_image ? SAVE_DEPTH_IMAGE : 0;
save_flags_ |= save_depth_camera_info ? SAVE_DEPTH_CAMERA_INFO : 0;
save_flags_ |= save_point_cloud ? SAVE_POINT_CLOUD : 0;
if (save_flags_ & SAVE_POINT_CLOUD)
cloud_sub_ = node_handle.subscribe("point_cloud", 1, &DataCollectionNode::pointCloudCallback, this);
if (save_flags_ & SAVE_IMAGE)
image_sub_ = image_transport_.subscribe("image", 1, &DataCollectionNode::imageCallback, this);
if (save_flags_ & SAVE_DEPTH_IMAGE)
depth_image_sub_ = image_transport_.subscribe("depth_image", 1, &DataCollectionNode::depthImageCallback, this);
if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)
camera_info_sub_ = node_handle.subscribe("camera_info", 1, &DataCollectionNode::cameraInfoCallback, this);
if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)
depth_camera_info_sub_ = node_handle.subscribe("depth_camera_info", 1, &DataCollectionNode::depthCameraInfoCallback, this);
std::string depth_type_s;
node_handle_.param("depth_type", depth_type_s, std::string("float32"));
if (depth_type_s == std::string("float32"))
depth_type_ = DEPTH_FLOAT32;
else if (depth_type_s == std::string("uint16"))
depth_type_ = DEPTH_UINT16;
else
ROS_FATAL_STREAM("[" << ros::this_node::getName() << "] Wrong \"depth_type\" parameter. Use \"float32\" or \"uint16\".");
}
DataCollectionNode::~DataCollectionNode()
{
// Do nothing
}
bool DataCollectionNode::initialize()
{
if (not waitForMessages())
return false;
ros::spinOnce();
if (search_checkerboard_)
{
for (size_t i = 0; i < checkerboard_array_msg_->checkerboards.size(); ++i)
cb_vec_.push_back(createCheckerboard(checkerboard_array_msg_->checkerboards[i], i));
}
return true;
}
void DataCollectionNode::pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr & msg)
{
cloud_msg_ = pcl::PCLPointCloud2::Ptr(new pcl::PCLPointCloud2());
pcl_conversions::toPCL(*msg, *cloud_msg_);
}
void DataCollectionNode::imageCallback(const sensor_msgs::Image::ConstPtr & msg)
{
image_msg_ = msg;
}
void DataCollectionNode::depthImageCallback(const sensor_msgs::Image::ConstPtr & msg)
{
depth_image_msg_ = msg;
}
void DataCollectionNode::cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg)
{
camera_info_msg_ = msg;
}
void DataCollectionNode::depthCameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr & msg)
{
depth_camera_info_msg_ = msg;
}
void DataCollectionNode::actionCallback(const Acquisition::ConstPtr & msg)
{
try
{
std::stringstream info_file_name;
info_file_name << save_folder_ << "info.yaml";
std::stringstream file_index_ss;
file_index_ss << std::setw(4) << std::setfill('0') << file_index_;
std::ofstream file;
file.open(info_file_name.str().c_str(), file_index_ == 1 ? std::ios_base::out : std::ios_base::out | std::ios_base::app);
if (file_index_ == 1)
file << "data:" << std::endl;
file << " - id: " << file_index_ss.str() << std::endl;
file << " timestamp_image: " << std::setprecision(19) << image_msg_->header.stamp.toSec() << std::setprecision(6) << std::endl;
file << " timestamp_depth: " << std::setprecision(19) << depth_image_msg_->header.stamp.toSec() << std::setprecision(6) << std::endl;
//file << " distance: " << msg->distance << std::endl;
//file << " info: \"" << msg->info << "\"" << std::endl;
file.close();
if (file_index_ == 1)
{
if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)
{
std::stringstream camera_info_file_name;
camera_info_file_name << save_folder_ << image_filename_ << "camera_info.yaml";
save(camera_info_msg_, camera_info_file_name.str());
}
if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)
{
std::stringstream depth_camera_info_file_name;
depth_camera_info_file_name << save_folder_ << depth_filename_ << "camera_info.yaml";
save(depth_camera_info_msg_, depth_camera_info_file_name.str());
}
}
if (save_flags_ & SAVE_POINT_CLOUD)
{
std::stringstream cloud_file_name;
cloud_file_name << save_folder_ << cloud_filename_ << file_index_ss.str() << ".pcd";
save(cloud_msg_, cloud_file_name.str());
}
if (save_flags_ & SAVE_IMAGE)
{
cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(image_msg_);
std::stringstream image_file_name;
image_file_name << save_folder_ << image_filename_ << file_index_ss.str() << "." << image_extension_;
save(image_ptr->image, image_file_name.str());
}
if (save_flags_ & SAVE_DEPTH_IMAGE)
{
cv_bridge::CvImage::Ptr depth_image_ptr;
if (depth_type_ == DEPTH_FLOAT32)
depth_image_ptr = cv_bridge::toCvCopy(depth_image_msg_, sensor_msgs::image_encodings::TYPE_32FC1);
else if (depth_type_ == DEPTH_UINT16)
depth_image_ptr = cv_bridge::toCvCopy(depth_image_msg_, sensor_msgs::image_encodings::TYPE_16UC1);
std::stringstream depth_file_name;
depth_file_name << save_folder_ << depth_filename_ << file_index_ss.str() << "." << image_extension_;
saveDepth(depth_image_ptr->image, depth_file_name.str());
}
ROS_INFO_STREAM_THROTTLE(1, "[" << ros::this_node::getName() << "] " << file_index_ss.str() << " saved");
file_index_++;
}
catch (cv_bridge::Exception & ex)
{
ROS_ERROR_STREAM("[" << ros::this_node::getName() << "] cv_bridge exception: " << ex.what());
}
}
void DataCollectionNode::save(const sensor_msgs::CameraInfo::ConstPtr & camera_info,
const std::string & file_name)
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(camera_info);
std::ofstream file;
file.open(file_name.c_str());
file << "frame_id: " << camera_info_msg_->header.frame_id << std::endl;
file << "height: " << camera_info_msg_->height << std::endl;
file << "width: " << camera_info_msg_->width << std::endl;
file << "distortion_model: " << camera_info_msg_->distortion_model << std::endl;
file << "D: " << model.distortionCoeffs() << std::endl;
file << "K: " << model.intrinsicMatrix().reshape<1, 9>() << std::endl;
file << "R: " << model.rotationMatrix().reshape<1, 9>() << std::endl;
file << "P: " << model.projectionMatrix().reshape<1, 12>() << std::endl;
file << "binning_x: " << camera_info_msg_->binning_x << std::endl;
file << "binning_y: " << camera_info_msg_->binning_y << std::endl;
file << "roi:" << std::endl;
file << " x_offset: " << camera_info_msg_->roi.x_offset << std::endl;
file << " y_offset: " << camera_info_msg_->roi.y_offset << std::endl;
file << " height: " << camera_info_msg_->roi.height << std::endl;
file << " width: " << camera_info_msg_->roi.width << std::endl;
file << " do_rectify: " << (camera_info_msg_->roi.do_rectify ? "True" : "False") << std::endl;
file.close();
}
void DataCollectionNode::checkerboardArrayCallback(const calibration_msgs::CheckerboardArray::ConstPtr & msg)
{
checkerboard_array_msg_ = msg;
}
bool DataCollectionNode::waitForMessages()
{
ROS_INFO_STREAM("[" << ros::this_node::getName() << "] Waiting for sensors...");
bool ret = true;
if (search_checkerboard_)
ret = ret and ros::topic::waitForMessage<CheckerboardArray>("checkerboard_array", node_handle_);
if (save_flags_ & SAVE_IMAGE)
ret = ret and ros::topic::waitForMessage<sensor_msgs::Image>("image", node_handle_);
if (save_flags_ & SAVE_POINT_CLOUD)
ret = ret and ros::topic::waitForMessage<sensor_msgs::PointCloud2>("point_cloud", node_handle_);
if (save_flags_ & SAVE_IMAGE_CAMERA_INFO)
ret = ret and ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", node_handle_);
if (save_flags_ & SAVE_DEPTH_IMAGE)
ret = ret and ros::topic::waitForMessage<sensor_msgs::Image>("depth_image", node_handle_);
if (save_flags_ & SAVE_DEPTH_CAMERA_INFO)
ret = ret and ros::topic::waitForMessage<sensor_msgs::CameraInfo>("depth_camera_info", node_handle_);
if (ret)
ROS_INFO_STREAM("[" << ros::this_node::getName() << "] All sensors connected");
else
ROS_ERROR_STREAM("[" << ros::this_node::getName() << "] Not all sensors connected!");
return ret;
}
void DataCollectionNode::save(const pcl::PCLPointCloud2::ConstPtr & cloud,
const std::string & file_name)
{
pcl::PCDWriter pcd_writer;
pcd_writer.writeBinary(file_name, *cloud);
}
void DataCollectionNode::save(const cv::Mat & image,
const std::string & file_name)
{
cv::imwrite(file_name, image);
}
void DataCollectionNode::saveDepth(const cv::Mat & depth_image,
const std::string & file_name)
{
if (depth_type_ == DEPTH_FLOAT32)
{
cv::Mat depth_image_16;
depth_image.convertTo(depth_image_16, CV_16UC1, 1000);
cv::imwrite(file_name, depth_image_16);
}
else if (depth_type_ == DEPTH_UINT16)
{
cv::imwrite(file_name, depth_image);
}
else
{
// Do nothing
}
}
Checkerboard::Ptr DataCollectionNode::createCheckerboard(const CheckerboardMsg::ConstPtr & msg,
int id)
{
return createCheckerboard(*msg, id);
}
Checkerboard::Ptr DataCollectionNode::createCheckerboard(const CheckerboardMsg & msg,
int id)
{
std::stringstream ss;
ss << "/checkerboard_" << id;
Checkerboard::Ptr cb = boost::make_shared<Checkerboard>(msg.cols, msg.rows, msg.cell_width, msg.cell_height);
cb->setFrameId(ss.str());
return cb;
}
} /* namespace calibration */
using namespace calibration;
int main(int argc,
char ** argv)
{
ros::init(argc, argv, "rgbd_data_collector");
ros::NodeHandle node_handle("~");
try
{
DataCollectionNode collector_node(node_handle);
if (not collector_node.initialize())
return 0;
ros::spin();
}
catch (std::runtime_error & error)
{
ROS_FATAL("Calibration error: %s", error.what());
return 1;
}
return 0;
}
================================================
FILE: src/rgbd_calibration/depth_undistortion_estimation.cpp
================================================
/*
* Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holder(s) nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <omp.h>
#include <algorithm>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <calibration_common/ceres/polynomial_fit.h>
#include <calibration_common/ceres/plane_fit.h>
#include <calibration_common/base/pcl_conversion.h>
#include <rgbd_calibration/checkerboard_views.h>
#include <calibration_common/algorithms/plane_extraction.h>
#include <rgbd_calibration/depth_undistortion_estimation.h>
#define RGBD_INFO(id, msg) ROS_INFO_STREAM("RGBD " << id << ": " << msg)
#define RGBD_WARN(id, msg) ROS_WARN_STREAM("RGBD " << id << ": " << msg)
namespace calibration
{
bool DepthUndistortionEstimation::extractPlane(const Checkerboard & color_cb,
const PCLCloud3::ConstPtr & cloud,
const Point3 & center,
PlaneInfo & plane_info)
{
Scalar radius = std::min(color_cb.width(), color_cb.height()) / 1.5; // TODO Add parameter
PointPlaneExtraction<PCLPoint3> plane_extractor;
plane_extractor.setInputCloud(cloud);
plane_extractor.setRadius(radius);
bool plane_extracted = false;
int r[] = {0, 1, 2}; // TODO Add parameter
int k[] = {0, 1, -1, 2, -2}; // TODO Add parameter
for (Size1 i = 0; i < 5 and not plane_extracted; ++i)
{
for (Size1 j = 0; j < 3 and not plane_extracted; ++j)
{
plane_extractor.setRadius((1 + r[j]) * radius);
plane_extractor.setPoint(PCLPoint3(center.x(), center.y(), center.z() + (1 + r[j]) * radius * k[i]));
plane_extracted = plane_extractor.extract(plane_info);
}
}
return plane_extracted;
}
void DepthUndistortionEstimation::estimateLocalModel()
{
std::sort(data_vec_.begin(), data_vec_.end(), OrderByDistance());
for (Size1 i = 0; i < data_vec_.size(); i += max_threads_)
{
#pragma omp parallel for schedule(static, 1)
for (Size1 th = 0; th < max_threads_; ++th)
{
if (i + th >= data_vec_.size())
continue;
const DepthData & data = *data_vec_[i + th];
const Checkerboard & gt_cb = *data.checkerboard_;
const PCLCloud3 & cloud = *data.cloud_;
// Estimate center
Point3 und_color_cb_center = gt_cb.center();
#pragma omp critical
{
InverseGlobalMatrixEigen inverse_global(inverse_global_fit_->model());
inverse_global.undistort(0, 0, und_color_cb_center);
}
// RGBD_INFO(data.id_, "Transformed z: " << gt_cb.center().z() << " -> " << und_color_cb_center.z());
// Undistort cloud
PCLCloud3::Ptr und_cloud = boost::make_shared<PCLCloud3>(cloud);
#pragma omp critical
{
LocalMatrixPCL local(local_fit_->model());
local.undistort(*und_cloud);
}
// Extract plane from undistorted cloud
PlaneInfo plane_info;
if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))
{
// RGBD_INFO(data.id_, "Plane extracted!!");
plane_info_map_[data_vec_[i + th]] = plane_info;
if ((i + th) % 10 == 0)
{
PCLCloud3::Ptr tmp_und_cloud = boost::make_shared<PCLCloud3>(*und_cloud, *plane_info.indices_);
PCLCloud3::Ptr tmp_cloud = boost::make_shared<PCLCloud3>(cloud, *plane_info.indices_);
std::stringstream ss;
pcl::PCDWriter writer;
ss << "/tmp/tmp_und_cloud_" << i + th << ".pcd";
writer.write(ss.str(), *und_cloud);
ss.str("");
ss << "/tmp/tmp_und_cloud_" << i + th << "_plane.pcd";
writer.write(ss.str(), *tmp_und_cloud);
ss.str("");
ss << "/tmp/tmp_cloud_" << i + th << ".pcd";
writer.write(ss.str(), cloud);
ss.str("");
ss << "/tmp/tmp_cloud_" << i + th << "_plane.pcd";
writer.write(ss.str(), *tmp_cloud);
PlaneInfo tmp_plane_info;
if (extractPlane(gt_cb, data.cloud_, und_color_cb_center, tmp_plane_info))
{
PCLCloud3::Ptr tmp_cloud_2 = boost::make_shared<PCLCloud3>(cloud, *tmp_plane_info.indices_);
ss.str("");
ss << "/tmp/tmp_cloud_" << i + th << "_plane_2.pcd";
writer.write(ss.str(), *tmp_cloud_2);
std::cout << plane_info.std_dev_ << " -- " << tmp_plane_info.std_dev_ << std::endl;
}
}
// Plane fitted_plane = PlaneFit<Scalar>::robustFit(PCLConversion<Scalar>::toPointMatrix(*und_cloud, *plane_info.indices_),
// plane_info.std_dev_);
std::vector<int> indices;// = *plane_info.indices_;
indices.reserve(plane_info.indices_->size());
int w = und_cloud->width;
int h = und_cloud->height;
for (size_t j = 0; j < plane_info.indices_->size(); ++j)
{
int r = (*plane_info.indices_)[j] / w;
int c = (*plane_info.indices_)[j] % w;
if ((r - h/2)*(r - h/2) + (c - w/2)*(c - w/2) < (h/2)*(h/2))
indices.push_back((*plane_info.indices_)[j]);
}
Plane fitted_plane = PlaneFit<Scalar>::fit(PCLConversion<Scalar>::toPointMatrix(cloud, indices)/*, plane_info.std_dev_*/);
plane_info_map_[data_vec_[i + th]].plane_ = fitted_plane;
#pragma omp critical
{
local_fit_->accumulateCloud(cloud, *plane_info.indices_);
local_fit_->addAccumulatedPoints(fitted_plane);
for (Size1 c = 0; c < gt_cb.corners().elements(); ++c)
{
const Point3 & corner = gt_cb.corners()[c];
inverse_global_fit_->addPoint(0, 0, corner, fitted_plane);
}
if (i + th > 20)
inverse_global_fit_->update();
}
Line line(Vector3::Zero(), gt_cb.center().normalized());
RGBD_INFO(data.id_, "Transformed z: " << gt_cb.center().z() << " -> " << und_color_cb_center.z()
<< " (Real z: " << line.intersectionPoint(fitted_plane).z() << ")");
// Scalar angle = RAD2DEG(std::acos(plane_info.equation_.normal().dot(gt_cb.plane().normal())));
// RGBD_INFO(data.id(), "Angle: " << angle);
}
else
RGBD_WARN(data.id_, "Plane not extracted!!");
}
local_fit_->update();
}
}
void DepthUndistortionEstimation::estimateLocalModelReverse()
{
//std::reverse(data_vec_.begin(), data_vec_.end());
local_fit_->reset();
for (Size1 i = 0; i < data_vec_.size(); i += max_threads_)
{
#pragma omp parallel for schedule(static, 1)
for (Size1 th = 0; th < max_threads_; ++th)
{
if (i + th >= data_vec_.size())
continue;
const DepthData & data = *data_vec_[i + th];
const Checkerboard & gt_cb = *data.checkerboard_;
const PCLCloud3 & cloud = *data.cloud_;
// Estimate center
Point3 und_color_cb_center = gt_cb.center();
#pragma omp critical
{
InverseGlobalMatrixEigen inverse_global(inverse_global_fit_->model());
inverse_global.undistort(0, 0, und_color_cb_center);
}
// RGBD_INFO(data.id_, "Transformed z: " << gt_cb.center().z() << " -> " << und_color_cb_center.z());
// Undistort cloud
PCLCloud3::Ptr und_cloud = boost::make_shared<PCLCloud3>(cloud);
#pragma omp critical
{
LocalMatrixPCL local(local_fit_->model());
local.undistort(*und_cloud);
}
// Extract plane from undistorted cloud
PlaneInfo plane_info;
if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))
{
// RGBD_INFO(data.id_, "Plane extracted!!");
// Plane fitted_plane = PlaneFit<Scalar>::robustFit(PCLConversion<Scalar>::toPointMatrix(*und_cloud, *plane_info.indices_),
// plane_info.std_dev_);
boost::shared_ptr<std::vector<int> > indices = boost::make_shared<std::vector<int> >();// = *plane_info.indices_;
indices->reserve(plane_info.indices_->size());
int w = und_cloud->width;
int h = und_cloud->height;
for (size_t j = 0; j < plane_info.indices_->size(); ++j)
{
int r = (*plane_info.indices_)[j] / w;
int c = (*plane_info.indices_)[j] % w;
if ((r - h/2)*(r - h/2) + (c - w/2)*(c - w/2) < (h/2)*(h/2))
indices->push_back((*plane_info.indices_)[j]);
}
Plane fitted_plane = PlaneFit<Scalar>::fit(PCLConversion<Scalar>::toPointMatrix(cloud, *indices)/*, plane_info.std_dev_*/);
boost::shared_ptr<std::vector<int> > old_indices;
#pragma omp critical
{
old_indices = plane_info_map_[data_vec_[i + th]].indices_;
}
indices->clear();
std::set_union(old_indices->begin(), old_indices->end(), plane_info.indices_->begin(), plane_info.indices_->end(), std::back_inserter(*indices));
#pragma omp critical
{
local_fit_->accumulateCloud(cloud, *indices);
local_fit_->addAccumulatedPoints(fitted_plane);
plane_info_map_[data_vec_[i + th]].indices_ = indices;
}
Line line(Vector3::Zero(), gt_cb.center().normalized());
RGBD_INFO(data.id_, "Transformed z: " << gt_cb.center().z() << " -> " << und_color_cb_center.z()
<< " (Real z: " << line.intersectionPoint(fitted_plane).z() << ")");
}
else
RGBD_WARN(data.id_, "Plane not extracted!!");
}
}
local_fit_->update();
//std::reverse(data_vec_.begin(), data_vec_.end());
}
class LocalModelError
{
public:
LocalModelError(const PCLCloud3::ConstPtr & cloud,
const Plane & plane,
const LocalModel::Ptr & local_model,
const Polynomial<double, 2> & depth_error_function)
: cloud_(cloud), plane_(plane), depth_error_function_(depth_error_function)
{
local_matrix_.setModel(local_model);
}
void addIndex(int x_index, int y_index)
{
indices_.push_back(std::make_pair(x_index, y_index));
}
void createCloudWithIndices()
{
part_cloud_.height = 1;
part_cloud_.width = indice
gitextract_t12mjzuf/
├── CMakeLists.txt
├── README.md
├── conf/
│ ├── checkerboards.yaml
│ ├── checkerboards_pepper.yaml
│ └── sim_camera.yaml
├── include/
│ └── rgbd_calibration/
│ ├── calibration.h
│ ├── calibration_node.h
│ ├── calibration_test.h
│ ├── checkerboard_views.h
│ ├── checkerboard_views_extractor.h
│ ├── depth_undistortion_estimation.h
│ ├── globals.h
│ ├── offline_calibration_node.h
│ ├── plane_based_extrinsic_calibration.h
│ ├── publisher.h
│ ├── simulation_node.h
│ └── test_node.h
├── launch/
│ ├── kinect2_507_tro.launch
│ ├── kinect2_507_tro_test.launch
│ ├── kinect_47A_tro.launch
│ ├── kinect_47A_tro_test.launch
│ ├── kinect_data_collection.launch
│ └── pepper_tro.launch
├── msg/
│ └── Acquisition.msg
├── package.xml
├── src/
│ └── rgbd_calibration/
│ ├── calibration.cpp
│ ├── calibration_node.cpp
│ ├── calibration_test.cpp
│ ├── checkerboard_views.cpp
│ ├── checkerboard_views_extractor.cpp
│ ├── data_collection_node.cpp
│ ├── depth_undistortion_estimation.cpp
│ ├── offline_calibration_node.cpp
│ ├── publisher.cpp
│ ├── simulation_node.cpp
│ └── test_node.cpp
└── test/
└── polynomial_undistortion_matrix_multifit_test.cpp
SYMBOL INDEX (52 symbols across 24 files)
FILE: include/rgbd_calibration/calibration.h
function namespace (line 41) | namespace calibration
FILE: include/rgbd_calibration/calibration_node.h
function namespace (line 44) | namespace calibration
FILE: include/rgbd_calibration/calibration_test.h
function namespace (line 32) | namespace calibration
FILE: include/rgbd_calibration/checkerboard_views.h
function namespace (line 41) | namespace calibration
FILE: include/rgbd_calibration/checkerboard_views_extractor.h
function namespace (line 35) | namespace calibration
FILE: include/rgbd_calibration/depth_undistortion_estimation.h
function namespace (line 39) | namespace calibration
function setDepthErrorFunction (line 170) | void setDepthErrorFunction(const Polynomial<Scalar, 2> & depth_error_fun...
FILE: include/rgbd_calibration/globals.h
function namespace (line 42) | namespace calibration
FILE: include/rgbd_calibration/offline_calibration_node.h
function namespace (line 23) | namespace calibration
FILE: include/rgbd_calibration/plane_based_extrinsic_calibration.h
function namespace (line 25) | namespace calibration
FILE: include/rgbd_calibration/publisher.h
function namespace (line 25) | namespace calibration
FILE: include/rgbd_calibration/simulation_node.h
function namespace (line 23) | namespace calibration
FILE: include/rgbd_calibration/test_node.h
function namespace (line 33) | namespace calibration
FILE: src/rgbd_calibration/calibration.cpp
type calibration (line 52) | namespace calibration
class CheckerboardDistanceConstraint (line 55) | class CheckerboardDistanceConstraint : public Constraint<Checkerboard>
method CheckerboardDistanceConstraint (line 60) | CheckerboardDistanceConstraint (Scalar distance,
method isValid (line 74) | inline virtual bool
class TransformError (line 321) | class TransformError
method TransformError (line 325) | TransformError(const PinholeCameraModel::ConstPtr & camera_model,
class ReprojectionError (line 434) | class ReprojectionError
method ReprojectionError (line 438) | ReprojectionError (const PinholeCameraModel::ConstPtr & camera_model,
method toEigen (line 449) | typename Types<T>::Pose toEigen(const T * const q, const T * const t...
class TransformDistortionError (line 484) | class TransformDistortionError
method TransformDistortionError (line 488) | TransformDistortionError(const PinholeCameraModel::ConstPtr & camera...
method toEigen (line 507) | typename Types<T>::Pose toEigen(const T * const q, const T * const t...
FILE: src/rgbd_calibration/calibration_node.cpp
type calibration (line 17) | namespace calibration
FILE: src/rgbd_calibration/calibration_test.cpp
type calibration (line 41) | namespace calibration
class NormalError (line 634) | class NormalError
method NormalError (line 638) | NormalError(const PinholeCameraModel::ConstPtr & camera_model,
FILE: src/rgbd_calibration/checkerboard_views.cpp
type calibration (line 23) | namespace calibration
FILE: src/rgbd_calibration/checkerboard_views_extractor.cpp
type calibration (line 35) | namespace calibration
function Size1 (line 38) | Size1 CheckerboardViewsExtraction::extract(const RGBDData::ConstPtr & ...
function Size1 (line 131) | Size1 CheckerboardViewsExtraction::extract(std::vector<CheckerboardVie...
function Size1 (line 137) | Size1 CheckerboardViewsExtraction::extractAll(std::vector<Checkerboard...
FILE: src/rgbd_calibration/data_collection_node.cpp
type calibration (line 32) | namespace calibration
class DataCollectionNode (line 35) | class DataCollectionNode
type DepthType (line 50) | enum DepthType
function main (line 468) | int main(int argc,
FILE: src/rgbd_calibration/depth_undistortion_estimation.cpp
type calibration (line 47) | namespace calibration
class LocalModelError (line 293) | class LocalModelError
method LocalModelError (line 297) | LocalModelError(const PCLCloud3::ConstPtr & cloud,
method addIndex (line 306) | void addIndex(int x_index, int y_index)
method createCloudWithIndices (line 311) | void createCloudWithIndices()
method size (line 323) | int size() const
FILE: src/rgbd_calibration/offline_calibration_node.cpp
type calibration (line 30) | namespace calibration
function main (line 248) | int
FILE: src/rgbd_calibration/publisher.cpp
type calibration (line 25) | namespace calibration
FILE: src/rgbd_calibration/simulation_node.cpp
type calibration (line 28) | namespace calibration
function main (line 208) | int main(int argc,
FILE: src/rgbd_calibration/test_node.cpp
type calibration (line 19) | namespace calibration
function main (line 522) | int main(int argc,
FILE: test/polynomial_undistortion_matrix_multifit_test.cpp
function main (line 87) | int main(int argc,
Condensed preview — 37 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (255K chars).
[
{
"path": "CMakeLists.txt",
"chars": 6047,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(rgbd_calibration)\nset(CMAKE_BUILD_TYPE RelWithDebInfo)\n\n## Find catkin mac"
},
{
"path": "README.md",
"chars": 589,
"preview": "## Dependencies\n\nThis package depends on [calibration_toolkit](https://github.com/iaslab-unipd/calibration_toolkit) vers"
},
{
"path": "conf/checkerboards.yaml",
"chars": 271,
"preview": "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 "
},
{
"path": "conf/checkerboards_pepper.yaml",
"chars": 74,
"preview": "checkerboards:\n- rows: 7\n cols: 9\n cell_width: 0.02\n cell_height: 0.02\n"
},
{
"path": "conf/sim_camera.yaml",
"chars": 424,
"preview": "image_width: 640\nimage_height: 480\ncamera_name: sim_camera\ncamera_matrix:\n rows: 3\n cols: 3\n data: [ 525, 0, 320, "
},
{
"path": "include/rgbd_calibration/calibration.h",
"chars": 6118,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "include/rgbd_calibration/calibration_node.h",
"chars": 3470,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "include/rgbd_calibration/calibration_test.h",
"chars": 3085,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/checkerboard_views.h",
"chars": 3984,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "include/rgbd_calibration/checkerboard_views_extractor.h",
"chars": 4126,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "include/rgbd_calibration/depth_undistortion_estimation.h",
"chars": 6823,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "include/rgbd_calibration/globals.h",
"chars": 3956,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/offline_calibration_node.h",
"chars": 1473,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/plane_based_extrinsic_calibration.h",
"chars": 3197,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/publisher.h",
"chars": 1864,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/simulation_node.h",
"chars": 1230,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "include/rgbd_calibration/test_node.h",
"chars": 2851,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "launch/kinect2_507_tro.launch",
"chars": 2893,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n<arg name=\"ns\" default=\"calibration\" />\n\n<group ns=\"$(arg ns)\">\n\n <arg name=\"path\" "
},
{
"path": "launch/kinect2_507_tro_test.launch",
"chars": 3144,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n<arg name=\"ns\" default=\"calibration\" />\n<arg name=\"dir\" default=\"$(env HOME)/Desktop/da"
},
{
"path": "launch/kinect_47A_tro.launch",
"chars": 2899,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n<arg name=\"ns\" default=\"calibration\" />\n\n<group ns=\"$(arg ns)\">\n\n <arg name=\"path\" "
},
{
"path": "launch/kinect_47A_tro_test.launch",
"chars": 3047,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n<arg name=\"ns\" default=\"calibration\" />\n<arg name=\"dir\" default=\"$(env HOME)/Desktop/da"
},
{
"path": "launch/kinect_data_collection.launch",
"chars": 2648,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"save_folder\" />\n <arg name=\"image_extension\" default=\"png\" />\n "
},
{
"path": "launch/pepper_tro.launch",
"chars": 2957,
"preview": "<?xml version=\"1.0\"?>\r\n<launch>\r\n\r\n<arg name=\"ns\" default=\"calibration\" />\r\n\r\n<group ns=\"$(arg ns)\">\r\n <arg name=\"pat"
},
{
"path": "msg/Acquisition.msg",
"chars": 29,
"preview": "float32 distance\nstring info\n"
},
{
"path": "package.xml",
"chars": 2907,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>rgbd_calibration</name>\n <version>0.0.0</version>\n <description>The calibratio"
},
{
"path": "src/rgbd_calibration/calibration.cpp",
"chars": 31190,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "src/rgbd_calibration/calibration_node.cpp",
"chars": 6961,
"preview": "#include <ros/ros.h>\n\n#include <pcl/io/pcd_io.h>\n\n#include <eigen_conversions/eigen_msg.h>\n\n#include <calibration_common"
},
{
"path": "src/rgbd_calibration/calibration_test.cpp",
"chars": 51122,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "src/rgbd_calibration/checkerboard_views.cpp",
"chars": 3991,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "src/rgbd_calibration/checkerboard_views_extractor.cpp",
"chars": 4933,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "src/rgbd_calibration/data_collection_node.cpp",
"chars": 15403,
"preview": "#include <ros/ros.h>\n#include <ros/topic.h>\n\n#include <pcl/io/pcd_io.h>\n#include <image_transport/image_transport.h>\n#in"
},
{
"path": "src/rgbd_calibration/depth_undistortion_estimation.cpp",
"chars": 18209,
"preview": "/*\n * Copyright (c) 2013-2014, Filippo Basso <bassofil@dei.unipd.it>\n *\n * All rights reserved.\n *\n * Redistribution "
},
{
"path": "src/rgbd_calibration/offline_calibration_node.cpp",
"chars": 8991,
"preview": "/*\n * offline_calibration_node.cpp\n *\n * Created on: Nov 28, 2012\n * Author: Filippo Basso\n */\n\n#include <boost/fi"
},
{
"path": "src/rgbd_calibration/publisher.cpp",
"chars": 4382,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "src/rgbd_calibration/simulation_node.cpp",
"chars": 6865,
"preview": "/*\n * Copyright (C) 2013 - Filippo Basso <bassofil@dei.unipd.it>\n *\n * This program is free software: you can redistri"
},
{
"path": "src/rgbd_calibration/test_node.cpp",
"chars": 17766,
"preview": "#include <ros/ros.h>\n\n#include <pcl/io/pcd_io.h>\n\n#include <eigen_conversions/eigen_msg.h>\n\n#include <calibration_common"
},
{
"path": "test/polynomial_undistortion_matrix_multifit_test.cpp",
"chars": 3339,
"preview": "#include <calibration_common/base/math.h>\n#include <rgbd_calibration/globals.h>\n#include <kinect/depth/polynomial_matrix"
}
]
About this extraction
This page contains the full source code of the iaslab-unipd/rgbd_calibration GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 37 files (237.6 KB), approximately 66.2k tokens, and a symbol index with 52 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.