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 = indices_.size();
    part_cloud_.points.resize(indices_.size());
    for (Size1 i = 0; i < indices_.size(); ++i)
    {
      int index = indices_[i].first + cloud_->width * indices_[i].second;
      part_cloud_.points[i] = cloud_->points[index];
    }
  }

  int size() const
  {
    return indices_.size();
  }

  bool operator ()(const double * const local_poly_1_data,
                   const double * const local_poly_2_data,
                   const double * const local_poly_3_data,
                   const double * const local_poly_4_data,
                   double * residuals) const
  {
    PCLCloud3 tmp_cloud = part_cloud_;

    Eigen::Map<Eigen::Matrix3Xd> residuals_map(residuals, 3, size());
    Eigen::MatrixX4d polynomials(MathTraits<LocalPolynomial>::Size, 4);
    polynomials.col(0) = Eigen::Map<const Eigen::Matrix<double, MathTraits<LocalPolynomial>::Size, 1> >(local_poly_1_data);
    polynomials.col(1) = Eigen::Map<const Eigen::Matrix<double, MathTraits<LocalPolynomial>::Size, 1> >(local_poly_2_data);
    polynomials.col(2) = Eigen::Map<const Eigen::Matrix<double, MathTraits<LocalPolynomial>::Size, 1> >(local_poly_3_data);
    polynomials.col(3) = Eigen::Map<const Eigen::Matrix<double, MathTraits<LocalPolynomial>::Size, 1> >(local_poly_4_data);

    for (Size1 i = 0; i < indices_.size(); ++i)
    {
      const std::pair<int, int> & index = indices_[i];

      std::vector<LocalModel::LookupTableData> lt_data = local_matrix_.model()->lookupTable(index.first, index.second);

      double tmp_depth = 0.0;
      double depth = tmp_cloud.points[i].z;
      for (Size1 j = 0; j < lt_data.size(); ++j)
        tmp_depth += lt_data[j].weight_ * LocalPolynomial::evaluate(polynomials.col(j), depth);
      depth = tmp_depth;
      double k = depth / tmp_cloud.points[i].z;

      Point3 p(tmp_cloud.points[i].x * k, tmp_cloud.points[i].y * k, depth);
      Line line(Vector3::Zero(), p.normalized());
//      residuals[i] = (p - line.intersectionPoint(plane_)).norm() / ceres::poly_eval(depth_error_function_.coefficients(), p.z());
      residuals_map.col(i) = (p - line.intersectionPoint(plane_)) / ceres::poly_eval(depth_error_function_.coefficients(), p.z());
    }

    return true;
  }

private:

  const PCLCloud3::ConstPtr cloud_;
  const Plane plane_;
  LocalMatrixPCL local_matrix_;
  const Polynomial<double, 2> depth_error_function_;
  std::vector<std::pair<int, int> > indices_;
  PCLCloud3 part_cloud_;

};

typedef ceres::NumericDiffCostFunction<LocalModelError, ceres::CENTRAL, ceres::DYNAMIC, MathTraits<LocalPolynomial>::Size,
MathTraits<LocalPolynomial>::Size, MathTraits<LocalPolynomial>::Size, MathTraits<LocalPolynomial>::Size> LocalCostFunction;

void DepthUndistortionEstimation::optimizeLocalModel(const Polynomial<double, 2> & depth_error_function)
{
  std::vector<LocalModelError *> all_errors_vec;
  ceres::Problem problem;

  for (Size1 i = 0; i < data_vec_.size(); ++i)
  {
    const DepthData::ConstPtr & data = data_vec_[i];
    const std::vector<int> & indices = *plane_info_map_[data].indices_;

    int delta_x = data->cloud_->width / local_model_->binSize().x();
    int delta_y = data->cloud_->height / local_model_->binSize().y();

    std::vector<LocalModelError *> error_vec;

    for (Size1 j = 0; j < delta_y * delta_x; ++j)
    {
      error_vec.push_back(new LocalModelError(data->cloud_, plane_info_map_[data].plane_, local_model_, depth_error_function));
      all_errors_vec.push_back(error_vec.back());
    }

    for (Size1 j = 0; j < indices.size(); ++j)
    {
      int x_index = indices[j] % data->cloud_->width;
      int y_index = indices[j] / data->cloud_->width;
      int bin_x = x_index / local_model_->binSize().x();
      int bin_y = y_index / local_model_->binSize().y();
      error_vec[bin_x + delta_x * bin_y]->addIndex(x_index, y_index);
    }

    for (Size1 j = 0; j < error_vec.size(); ++j)
    {
      if (error_vec[j]->size() == 0)
        continue;
      error_vec[j]->createCloudWithIndices();

      int x_index = j % delta_x;
      int y_index = j / delta_x;

//      ceres::CostFunction * cost_function = new LocalCostFunction(error_vec[j], ceres::DO_NOT_TAKE_OWNERSHIP, error_vec[j]->size());
      ceres::CostFunction * cost_function = new LocalCostFunction(error_vec[j], ceres::DO_NOT_TAKE_OWNERSHIP, 3 * error_vec[j]->size());
      problem.AddResidualBlock(cost_function, NULL,
                               local_model_->matrix()->at(x_index, y_index).data(),
                               local_model_->matrix()->at(x_index, y_index + 1).data(),
                               local_model_->matrix()->at(x_index + 1, y_index).data(),
                               local_model_->matrix()->at(x_index + 1, y_index + 1).data());

    }

  }


  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.max_num_iterations = 10;
  options.minimizer_progress_to_stdout = true;
  options.num_threads = 8;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  for (Size1 i = 0; i < all_errors_vec.size(); ++i)
    delete all_errors_vec[i];

}

void DepthUndistortionEstimation::estimateGlobalModel()
{
#pragma omp parallel for
  for (size_t i = 0; i < data_vec_.size(); ++i)
  {
    DepthData & data = *data_vec_[i];
    const Checkerboard & gt_cb = *data.checkerboard_;
    const PCLCloud3 & cloud = *data.cloud_;

    Point3 und_color_cb_center = gt_cb.center();
    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());

    PCLCloud3::Ptr und_cloud = boost::make_shared<PCLCloud3>(cloud);
    LocalMatrixPCL local(local_fit_->model());
    local.undistort(*und_cloud);

    PlaneInfo plane_info;

    if (extractPlane(gt_cb, und_cloud, und_color_cb_center, plane_info))
    {
      data.estimated_plane_ = plane_info;
      data.undistorted_cloud_ = und_cloud;
      data.plane_extracted_ = true;

#pragma omp critical
      {
        Indices reduced = *plane_info.indices_;
        std::random_shuffle(reduced.begin(), reduced.end());
        //reduced.resize(reduced.size() / 5);
        global_fit_->accumulateCloud(*und_cloud, reduced);
        global_fit_->addAccumulatedPoints(gt_cb.plane());
      }
    }
    else
      RGBD_WARN(data.id_, "Plane not extracted!!");

  }
  global_fit_->update();

}

} /* namespace calibration */


================================================
FILE: src/rgbd_calibration/offline_calibration_node.cpp
================================================
/*
 * offline_calibration_node.cpp
 *
 *  Created on: Nov 28, 2012
 *      Author: Filippo Basso
 */

#include <boost/filesystem.hpp>

#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/polynomial_matrix_io.h>
#include <rgbd_calibration/offline_calibration_node.h>

//#include <swissranger_camera/utility.h>
#include <pcl/conversions.h>

using namespace camera_info_manager;
using namespace calibration_msgs;

namespace fs = boost::filesystem;

namespace calibration
{

OfflineCalibrationNode::OfflineCalibrationNode (ros::NodeHandle & node_handle)
  : CalibrationNode(node_handle),
	starting_index_(0)
{
  if (not node_handle_.getParam("path", path_))
    ROS_FATAL("Missing \"path\" parameter!!");

  if (path_[path_.size() - 1] != '/')
    path_.append("/");

  if (not node_handle_.getParam("instances", instances_))
    ROS_FATAL("Missing \"instances\" parameter!!");

  node_handle_.param("image_filename", image_filename_, std::string("image_"));
  node_handle_.param("cloud_filename", cloud_filename_, std::string("cloud_"));

  std::string depth_type_s;
  node_handle_.param("depth_type", depth_type_s, std::string("none"));
  if (depth_type_s == "kinect1_depth")
    depth_type_ = KINECT1_DEPTH;
  else if (depth_type_s == "swiss_ranger_depth")
    depth_type_ = SWISS_RANGER_DEPTH;
  else
    ROS_FATAL("Missing \"depth_type\" parameter!! Use \"kinect1_depth\" or \"swiss_ranger_depth\"");
}

void
OfflineCalibrationNode::spin ()
{

  fs::path path(path_);
  fs::directory_iterator end_it;

  std::map<std::string, std::string> cloud_file_map, image_file_map;

  for (fs::directory_iterator it(path); it != end_it; ++it)
  {
    fs::path file = it->path();
    if (fs::is_regular_file(file))
    {
      std::string file_name = file.filename().string();

      if (file_name.substr(0, cloud_filename_.size()) == cloud_filename_)
      {
        std::string id = file_name.substr(cloud_filename_.size(), file_name.find_last_of('.') - cloud_filename_.size());
        cloud_file_map[id] = file.string();
      }
      else if (file_name.substr(0, image_filename_.size()) == image_filename_)
      {
        std::string id = file_name.substr(image_filename_.size(), file_name.find_last_of('.') - image_filename_.size());
        image_file_map[id] = file.string();
      }
    }
  }

  typedef std::map<std::string, std::string>::const_iterator map_iterator;

  int added = 0;

  ROS_INFO("Getting data...");
  for (map_iterator cloud_it = cloud_file_map.begin(); cloud_it != cloud_file_map.end() and added < instances_; ++cloud_it)
  {
    map_iterator image_it = image_file_map.find(cloud_it->first);
    if (image_it != image_file_map.end())
    {
      cv::Mat image = cv::imread(image_it->second);

      if (not image.data)
      {
        ROS_WARN_STREAM(image_it->second << " not valid!");
        continue;
      }

      PCLCloud3::Ptr cloud;

      pcl::PCDReader pcd_reader;
      if (depth_type_ == KINECT1_DEPTH)
      {
        cloud = boost::make_shared<PCLCloud3>();

        if (pcd_reader.read(cloud_it->second, *cloud) < 0)
        {
          ROS_WARN_STREAM(cloud_it->second << " not valid!");
          continue;
        }

        /*const Scalar & fx = depth_sensor_->cameraModel()->fx() * depth_sensor_->cameraModel()->binningX();
        const Scalar & fy = depth_sensor_->cameraModel()->fy() * depth_sensor_->cameraModel()->binningY();
        const Scalar & cx = depth_sensor_->cameraModel()->cx() * depth_sensor_->cameraModel()->binningX();
        const Scalar & cy = depth_sensor_->cameraModel()->cy() * depth_sensor_->cameraModel()->binningY();*/

        cv::Matx33d K = /*static_cast<int>(depth_sensor_->cameraModel()->binningX()) **/ depth_sensor_->cameraModel()->fullIntrinsicMatrix();

        for (int j = 0; j < cloud->height; ++j)
        {
          for (int k = 0; k < cloud->width; ++k)
          {
            /*cloud->at(k, j).x = (k - cx) * cloud->at(k, j).z / fx;
            cloud->at(k, j).y = (j - cy) * cloud->at(k, j).z / fy;*/
            Scalar z = cloud->at(k, j).z;
            Point2 normalized_pixel((k - K(0, 2)) / K(0, 0), (j - K(1, 2)) / K(1, 1));

            Point3 p = z * depth_sensor_->cameraModel()->undistort2d_<Scalar>(normalized_pixel).homogeneous();

            cloud->at(k, j).x = p.x();
            cloud->at(k, j).y = p.y();
          }
        }

        /*for (size_t v = 0; v < cloud->height; ++v)
        {
          for (size_t u = 0; u < cloud->width; ++u)
          {
            PCLPoint3 & pt = cloud->points[u + v * cloud->width];
//            pt.x = (u - 314.5) * pt.z / 575.8157348632812;
//            pt.y = (v - 235.5) * pt.z / 575.8157348632812;
//            pt.x = (u - 309.7947658766498) * pt.z / 584.3333129882812;
//            pt.y = (v - 245.9642466885198) * pt.z / 582.8702392578125;
            pt.x = (u - depth_sensor_->cameraModel()->cx()) * pt.z / depth_sensor_->cameraModel()->fx();
            pt.y = (v - depth_sensor_->cameraModel()->cy()) * pt.z / depth_sensor_->cameraModel()->fy();
          }
        }*/

//        pcl::PCDWriter pcd_writer;
//        pcd_writer.write(cloud_it->second + "_rev.pcd", *cloud);

      }
      else if (depth_type_ == SWISS_RANGER_DEPTH)
      {
//        pcl::PCLPointCloud2Ptr pcl_cloud = boost::make_shared<pcl::PCLPointCloud2>();
//        sensor_msgs::PointCloud2Ptr ros_cloud = boost::make_shared<sensor_msgs::PointCloud2>();
//        sr::Utility sr_utility;
//        if (pcd_reader.read(cloud_it->second, *pcl_cloud) < 0)
//        {
//          ROS_WARN_STREAM(cloud_it->second << " not valid!");
//          continue;
//        }
//        pcl_conversions::fromPCL(*pcl_cloud, *ros_cloud);
//
//        sr_utility.setConfidenceThreshold(0.90f);
//        sr_utility.setInputCloud(ros_cloud);
//        sr_utility.setIntensityType(sr::Utility::INTENSITY_8BIT);
//        sr_utility.setConfidenceType(sr::Utility::CONFIDENCE_8BIT);
//        sr_utility.setNormalizeIntensity(true);
//        sr_utility.split(sr::Utility::CLOUD);
//
//        cloud = sr_utility.cloud();
      }

      calibration_->addData(image, cloud);
      ++added;

      if (added == instances_)
        break;

      ROS_DEBUG_STREAM("(" << image_it->second << ", " << cloud_it->second << ") added.");
    }
  }

  ROS_INFO_STREAM(added << " images + point clouds added.");

  geometry_msgs::Pose pose_msg;
  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Initial transform:\n" << pose_msg);

  calibration_->perform();

  PolynomialUndistortionMatrixIO<LocalPolynomial> local_io;
  local_io.write(*calibration_->localModel(), path_ + "local_matrix.txt");

  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Estimated transform:\n" << pose_msg);

  calibration_->optimize();

  PolynomialUndistortionMatrixIO<GlobalPolynomial> global_io;
  global_io.write(*calibration_->globalModel(), path_ + "global_matrix.txt");

  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Optimized transform:\n" << pose_msg);

  std::ofstream transform_file;
  transform_file.open((path_ + "camera_pose.yaml").c_str());
  transform_file << pose_msg;
  transform_file.close();

  const std::vector<double> & depth_intrinsics = calibration_->optimizedIntrinsics();

  ROS_INFO_STREAM("fx = " << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[0]);
  ROS_INFO_STREAM("fy = " << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[1]);
  ROS_INFO_STREAM("cx = " << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[2]);
  ROS_INFO_STREAM("cy = " << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[3]);

  std::ofstream intrinsics_file;
  intrinsics_file.open((path_ + "depth_intrinsics.yaml").c_str());
  intrinsics_file << "intrinsics:" << std::endl;
  intrinsics_file << "  fx: " << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[0] << std::endl;
  intrinsics_file << "  fy: " << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[1] << std::endl;
  intrinsics_file << "  cx: " << depth_sensor_->cameraModel()->binningX() * depth_intrinsics[2] << std::endl;
  intrinsics_file << "  cy: " << depth_sensor_->cameraModel()->binningY() * depth_intrinsics[3] << std::endl;
  intrinsics_file.close();


//  ros::Rate rate(1.0);

//  while (ros::ok())
//  {
//    calibration_->publishData();
//    rate.sleep();
//  }

}

} /* namespace calibration */

int
main (int argc,
      char ** argv)
{
  ros::init(argc, argv, "offline_calibration");
  ros::NodeHandle node_handle("~");

  try
  {
    calibration::OfflineCalibrationNode calib_node(node_handle);
    if (not calib_node.initialize())
      return 0;
    calib_node.spin();
  }
  catch (const std::runtime_error & error)
  {
    ROS_FATAL_STREAM("Calibration error: " << error.what());
    return 1;
  }

  return 0;
}



================================================
FILE: src/rgbd_calibration/publisher.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 <pcl_ros/point_cloud.h>
#include <visualization_msgs/MarkerArray.h>
#include <eigen_conversions/eigen_msg.h>
#include <cv_bridge/cv_bridge.h>

#include <rgbd_calibration/publisher.h>

namespace calibration
{

Publisher::Publisher(const ros::NodeHandle & node_handle)
  : node_handle_(node_handle)
{
  marker_pub_ = node_handle_.advertise<visualization_msgs::Marker>("rgbd_markers", 0);
}

void Publisher::publishTF(const BaseObject & object)
{
  geometry_msgs::TransformStamped transform_msg;
  object.toTF(transform_msg);
  tf_pub_.sendTransform(transform_msg);
}

//void Publisher::publish(const RGBDCheckerboard & rgbd_checkerboard,
//                            const DistortionMap<double, PointT> & map)
//{
//  publish(rgbd_checkerboard);
//
//  visualization_msgs::Marker cloud_marker;
//  cloud_marker.ns = "undist_cloud";
//
//  Point3Matrix points = rgbd_checkerboard.depthView()->interestPoints();
//  map.undistort(points);
//
//  cloud_marker.header.stamp = ros::Time::now();
//  cloud_marker.header.frame_id = rgbd_checkerboard.depthView()->sensor()->frameId();
//  points.toMarker(cloud_marker);
//  cloud_marker.color.r = 0.0;
//  cloud_marker.color.g = 1.0;
//  cloud_marker.color.b = 0.0;
//
//  CheckerboardPublisherSet & pub_set = c_pub_map_[rgbd_checkerboard.id()];
//  pub_set.marker_pub_.publish(cloud_marker);
//}

void Publisher::publish(const CheckerboardViews & rgbd_checkerboard,
                        const std::string & prefix)
{
  std::stringstream ss;
  ss << prefix << "checkerboard_" << rgbd_checkerboard.id();
  std::string ns = ss.str();

  if (c_pub_map_.find(ns) == c_pub_map_.end())
  {
    CheckerboardPublisherSet pub_set;

    std::stringstream sss;
    sss << ns << "/markers";
    pub_set.marker_pub_ = node_handle_.advertise<visualization_msgs::Marker>(sss.str(), 0);

    c_pub_map_[ns] = pub_set;
  }

  visualization_msgs::Marker checkerboard_marker;
  visualization_msgs::Marker cloud_marker;
  visualization_msgs::Marker d_plane_marker;

  checkerboard_marker.ns = "checkerboard";
  cloud_marker.ns = "cloud";
  d_plane_marker.ns = "d_plane";

  checkerboard_marker.id = cloud_marker.id = d_plane_marker.id = 0;

  CheckerboardPublisherSet & pub_set = c_pub_map_[ns];

  if (rgbd_checkerboard.colorCheckerboard())
  {
    rgbd_checkerboard.colorCheckerboard()->toMarker(checkerboard_marker);
    pub_set.marker_pub_.publish(checkerboard_marker);
  }

//  if (rgbd_checkerboard.depthPlane())
//  {
//    rgbd_checkerboard.depthPlane()->toMarker(d_plane_marker);
//    pub_set.marker_pub_.publish(d_plane_marker);
//  }

//  if (rgbd_checkerboard.depthView())
//  {
//    rgbd_checkerboard.depthView()->toMarker(cloud_marker);
//    pub_set.marker_pub_.publish(cloud_marker);
//  }
}

void Publisher::publish(const RGBDData & rgbd)
{
  std::stringstream ss;
  ss << "rgbd_" << rgbd.id();
  std::string ns = ss.str();

  if (d_pub_map_.find(rgbd.id()) == d_pub_map_.end())
  {
    DataPublisherSet pub_set;

    ss.str("");
    ss << ns << "/cloud";
    pub_set.cloud_pub_ = node_handle_.advertise<PCLCloud3>(ss.str(), 0);

    //    ss.str("");
    //    ss << ns << "/image";
    //    pub_set.image_pub_ = node_handle_.advertise<sensor_msgs::Image>(ss.str(), 0);

    ss.str("");
    ss << ns << "/rgbd";
    pub_set.rgbd_pub_ = node_handle_.advertise<PCLCloudRGB>(ss.str(), 0);

    d_pub_map_[rgbd.id()] = pub_set;
  }

  DataPublisherSet & pub_set = d_pub_map_[rgbd.id()];

//  rgbd.depthData()->header.stamp = ros::Time::now().toNSec();
  pub_set.cloud_pub_.publish(*rgbd.depthData());
  rgbd.fuseData();
  pub_set.rgbd_pub_.publish(*rgbd.fusedData());

}

} /* namespace calibration */


================================================
FILE: src/rgbd_calibration/simulation_node.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/simulation_node.h>
#include <omp.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include <eigen_conversions/eigen_msg.h>

#include <pcl/visualization/pcl_visualizer.h>

namespace calibration
{

SimulationNode::SimulationNode(ros::NodeHandle & node_handle)
  : CalibrationNode(node_handle)
{

  //  if (not node_handle_.getParam("instances", instances_))
  //    ROS_FATAL("Missing \"instances\" parameter!!");

  node_handle_.param("depth_error", depth_error_, 0.0035);
  node_handle_.param("image_error", image_error_, 0.5);

  node_handle_.param("min_distance", min_distance_, 1.0);
  node_handle_.param("max_distance", max_distance_, 4.0);
  node_handle_.param("step", step_, 1.0);

}

void SimulationNode::spin()
{
  ros::Rate rate(10.0);

  int added = 0;

  Scalar aa = -0.01, bb = 0.96, cc = 0.0;
  GlobalPolynomial::Coefficients global_error_function_coeffs(cc, bb, aa);
  GlobalPolynomial global_error_function(global_error_function_coeffs);

  ROS_INFO_STREAM(global_error_function_coeffs.transpose());

  ROS_INFO("Getting data...");
  for (double z = min_distance_; z <= max_distance_ + 0.01; z += step_)
  {
    Translation3 translation_cb_center(-cb_vec_[0]->center());
    Translation3 translation_z(0, 0, z);

    boost::mt19937 random_gen(time(0));

    boost::normal_distribution<> depth_error(0.0, depth_error_);
    boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > depth_noise(random_gen, depth_error);

    boost::normal_distribution<> image_error(0.0, image_error_);
    boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > image_noise(random_gen, image_error);

#pragma omp parallel for
    for (int i = 0; i < 9; ++i)
    {
      Transform T = Transform::Identity();

      if (i % 3 != 1)
      {
        AngleAxis r;
        r.axis() = Vector3::UnitY();
        r.angle() = (i % 3 == 2 ? M_PI / 6 : -M_PI / 6);
        T.prerotate(r);
      }
      if (i / 3 != 1)
      {
        AngleAxis r;
        r.axis() = Vector3::UnitX();
        r.angle() = (i / 3 == 0 ? M_PI / 6 : -M_PI / 6);
        T.prerotate(r);
      }

      T = translation_z * T * translation_cb_center;

      Checkerboard::Ptr real_cb = boost::make_shared<Checkerboard>(*cb_vec_[0]);
      real_cb->transform(T);

      // Create image

      cv::Mat image(480, 640, CV_8UC3, cv::Scalar(255, 255, 255));
      Cloud2 image_corners = color_sensor_->cameraModel()->project3dToPixel(real_cb->corners());

      std::vector<cv::Point2f> corners;
      for (Size1 c = 0; c < image_corners.elements(); ++c)
      {
        if (image_corners[c].x() > 0 and image_corners[c].x() < image.cols and image_corners[c].y() > 0
            and image_corners[c].y() < image.rows)
          corners.push_back(cv::Point2f(image_corners[c].x(), image_corners[c].y()));

        image_corners[c].x() += image_noise();
        image_corners[c].y() += image_noise();

      }
      cv::drawChessboardCorners(image, cv::Size(image_corners.size().x(), image_corners.size().y()), corners,
                                corners.size() == image_corners.elements());

      //      cv::imshow("AAA", image);
      //      cv::waitKey(100);

      // Create point cloud

      PCLCloud3::Ptr cloud = boost::make_shared<PCLCloud3>();
      std::vector<int> inliers;

      real_cb->transform(color_sensor_->pose());

      for (int y = 0; y < 480; ++y)
      {
        for (int x = 0; x < 640; ++x)
        {
          Line line(Point3::Zero(),
                    color_sensor_->cameraModel()->projectPixelTo3dRay(Point2(x, y)));
          Point3 point = line.intersectionPoint(real_cb->plane());
          Scalar new_z = (-bb + std::sqrt(bb * bb - 4 * aa * (cc - point.z()))) / (2 * aa);
          new_z = new_z + depth_noise() * new_z * new_z;
          point *= new_z / point.z();
          PCLPoint3 pcl_point;
          pcl_point.x = point.x();
          pcl_point.y = point.y();
          pcl_point.z = point.z();
          cloud->points.push_back(pcl_point);
          inliers.push_back(inliers.size());
        }
      }

      cloud->width = 640;
      cloud->height = 480;

      //      pcl::visualization::PCLVisualizer vis;
      //      vis.addPointCloud(cloud);
      //      vis.spin();

      RGBDData::Ptr data = boost::make_shared<RGBDData>(i + z * 3);
      data->setColorSensor(color_sensor_);
      data->setDepthSensor(depth_sensor_);
      data->setColorData(image);
      data->setDepthData(*cloud);

      std::stringstream ss;
      ss << "view_" << data->id() << "_" << i;

      CheckerboardViews::Ptr cb_views = boost::make_shared<CheckerboardViews>(ss.str());
      cb_views->setData(data);
      cb_views->setCheckerboard(cb_vec_[0]);
      cb_views->setImageCorners(image_corners);
      //cb_views->setPlaneInliers(inliers, 0.01);

#pragma omp critical
      {
        calibration_->addCheckerboardViews(cb_views);
        ++added;
      }
    }

    //rate.sleep();
  }

  ROS_INFO_STREAM("Added " << added << " checkerboards.");

  geometry_msgs::Pose pose_msg;
  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Initial transform:\n" << pose_msg);

  calibration_->setEstimateDepthUndistortionModel(true);
  //calibration_->setEstimateDepthUndistortionModel(false);
  calibration_->perform();

  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Estimated transform:\n" << pose_msg);

  calibration_->optimize();

  tf::poseEigenToMsg(color_sensor_->pose(), pose_msg);
  ROS_INFO_STREAM("Optimized transform:\n" << pose_msg);

  rate = ros::Rate(1.0);

  while (ros::ok())
  {
    calibration_->publishData();
    rate.sleep();
  }

}

} /* namespace calibration */

int main(int argc,
         char ** argv)
{
  ros::init(argc, argv, "simulation");
  ros::NodeHandle node_handle("~");

  try
  {
    calibration::SimulationNode sim_node(node_handle);
    if (not sim_node.initialize())
      return 0;
    sim_node.spin();

    ros::spin();
  }
  catch (std::runtime_error & error)
  {
    ROS_FATAL("Calibration error: %s", error.what());
    return 1;
  }

  return 0;
}



================================================
FILE: src/rgbd_calibration/test_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/polynomial_matrix_io.h>

#include <rgbd_calibration/test_node.h>

//#include <swissranger_camera/utility.h>

using namespace camera_info_manager;
using namespace calibration_msgs;

namespace calibration
{

TestNode::TestNode(ros::NodeHandle & node_handle)
  : node_handle_(node_handle)
{
  checkerboards_sub_ = node_handle_.subscribe("checkerboard_array", 1, &TestNode::checkerboardArrayCallback, this);

  if (not node_handle_.getParam("camera_calib_url", camera_calib_url_))
    ROS_FATAL("Missing \"camera_calib_url\" parameter!!");
  node_handle_.param("camera_name", camera_name_, std::string("camera"));

  if (not node_handle_.getParam("depth_camera_calib_url", depth_camera_calib_url_))
      ROS_FATAL("Missing \"depth_camera_calib_url\" parameter!!");
  node_handle_.param("depth_camera_name", depth_camera_name_, std::string("depth_camera"));

  node_handle_.param("downsample_ratio", downsample_ratio_, 1);
  if (downsample_ratio_ < 1)
  {
    downsample_ratio_ = 1;
    ROS_WARN("\"downsample_ratio\" cannot be < 1. Skipping.");
  }

#if (!defined(UNCALIBRATED)) || (defined(UNCALIBRATED) && defined(SKEW))
  if (node_handle_.hasParam("camera_pose"))
  {
    Translation3 translation;
    Quaternion rotation;

    node_handle_.getParam("camera_pose/position/x", translation.vector().coeffRef(0));
    node_handle_.getParam("camera_pose/position/y", translation.vector().coeffRef(1));
    node_handle_.getParam("camera_pose/position/z", translation.vector().coeffRef(2));

    node_handle_.getParam("camera_pose/orientation/x", rotation.coeffs().coeffRef(0));
    node_handle_.getParam("camera_pose/orientation/y", rotation.coeffs().coeffRef(1));
    node_handle_.getParam("camera_pose/orientation/z", rotation.coeffs().coeffRef(2));
    node_handle_.getParam("camera_pose/orientation/w", rotation.coeffs().coeffRef(3));

    rotation.normalize();

    camera_pose_ = translation * rotation;
  }
  else
    ROS_FATAL("Missing \"camera_pose\" parameter!!");
#else
  camera_pose_ = Eigen::Affine3d::Identity() * Translation3(0.052, 0.0, 0.0);
#endif



  if (node_handle_.hasParam("local_und_matrix_file"))
    node_handle_.getParam("local_und_matrix_file", local_matrix_file_);
  else
    ROS_FATAL("Missing \"local_und_matrix_file\" parameter!!");

  if (node_handle_.hasParam("global_und_matrix_file"))
    node_handle_.getParam("global_und_matrix_file", global_matrix_file_);
  else
    ROS_FATAL("Missing \"global_und_matrix_file\" parameter!!");

  if (not node_handle_.getParam("path", path_))
    ROS_FATAL("Missing \"path\" parameter!!");

  if (path_[path_.size() - 1] != '/')
    path_.append("/");

  bool ok = true;
  if (not node_handle_.hasParam("intrinsics"))
    ROS_FATAL("Missing \"intrinsics\" parameter!!");
  depth_intrinsics_.resize(4);
  ok = ok and node_handle_.getParam("intrinsics/fx", depth_intrinsics_[0]);
  ok = ok and node_handle_.getParam("intrinsics/fy", depth_intrinsics_[1]);
  ok = ok and node_handle_.getParam("intrinsics/cx", depth_intrinsics_[2]);
  ok = ok and node_handle_.getParam("intrinsics/cy", depth_intrinsics_[3]);
#ifdef SKEW
  ok = ok and node_handle_.getParam("intrinsics/s", depth_intrinsics_[4]);
#endif
  if (not ok)
    ROS_FATAL("Malformed \"intrinsics\" parameter!!");

  if (not node_handle_.getParam("instances", instances_))
    ROS_FATAL("Missing \"instances\" parameter!!");

  node_handle_.param("image_extension", image_extension_, std::string("png"));
  node_handle_.param("starting_index", starting_index_, 1);
  node_handle_.param("image_filename", image_filename_, std::string("image_"));
  node_handle_.param("cloud_filename", cloud_filename_, std::string("cloud_"));

  node_handle_.param("only_show", only_show_, false);

  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 / downsample_ratio_;
  images_size_.y() = images_size_y / downsample_ratio_;

  std::string depth_type_s;
  node_handle_.param("depth_type", depth_type_s, std::string("none"));
  if (depth_type_s == "kinect1_depth")
    depth_type_ = KINECT1_DEPTH;
  else if (depth_type_s == "swiss_ranger_depth")
    depth_type_ = SWISS_RANGER_DEPTH;
  else
    ROS_FATAL("Missing \"depth_type\" parameter!! Use \"kinect1_depth\" or \"swiss_ranger_depth\"");

}

bool TestNode::initialize()
{
  if (not waitForMessages())
    return false;

  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 depth_manager(node_handle_, depth_camera_name_, depth_camera_calib_url_);
  sensor_msgs::CameraInfo msg = depth_manager.getCameraInfo();
  /*msg.P[0] = depth_intrinsics_[0];
  msg.P[5] = depth_intrinsics_[1];
  msg.P[2] = depth_intrinsics_[2];
  msg.P[6] = depth_intrinsics_[3];
  msg.K[0] = depth_intrinsics_[0];
  msg.K[4] = depth_intrinsics_[1];
  msg.K[2] = depth_intrinsics_[2];
  msg.K[5] = depth_intrinsics_[3];

#ifdef SKEW
  msg.P[1] = depth_intrinsics_[4];
  msg.K[1] = depth_intrinsics_[4];
#endif*/

  KinectDepthCameraModel::ConstPtr depth_pinhole_model = boost::make_shared<KinectDepthCameraModel>(msg);

  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(KINECT_ERROR_POLY); // TODO add parameter
//  depth_sensor_->setDepthErrorFunction(depth_error_function);

  CameraInfoManager manager(node_handle_, camera_name_, camera_calib_url_);
  msg = manager.getCameraInfo();

  PinholeCameraModel::ConstPtr pinhole_model = boost::make_shared<PinholeCameraModel>(manager.getCameraInfo());
  cv::Mat P = cv::getOptimalNewCameraMatrix(depth_pinhole_model->fullIntrinsicMatrix(), depth_pinhole_model->distortionCoeffs(),
                                            depth_pinhole_model->fullResolution(), 0);

  std::cout << P << std::endl;

  color_sensor_ = boost::make_shared<PinholeSensor>();
  color_sensor_->setFrameId("/pinhole_sensor");
  color_sensor_->setCameraModel(pinhole_model);
  color_sensor_->setParent(depth_sensor_);
  color_sensor_->transform(camera_pose_);

  LocalModel::Data::Ptr local_und_data;

  PolynomialUndistortionMatrixIO<LocalPolynomial> local_io;
  if (not local_io.read(local_und_data, local_matrix_file_))
    ROS_FATAL_STREAM("File " << local_matrix_file_ << " not found!!");

  GlobalModel::Data::Ptr global_data;

  PolynomialUndistortionMatrixIO<GlobalPolynomial> global_io;
  if (not global_io.read(global_data, global_matrix_file_))
    ROS_FATAL_STREAM("File " << global_matrix_file_ << " not found!!");

  LocalModel::Ptr local_model = boost::make_shared<LocalModel>(images_size_);
  local_model->setMatrix(local_und_data);

  LocalMatrixPCL::Ptr local_und = boost::make_shared<LocalMatrixPCL>();
  local_und->setModel(local_model);

  GlobalModel::Ptr global_model = boost::make_shared<GlobalModel>(images_size_);
  global_model->setMatrix(global_data);

  GlobalMatrixPCL::Ptr global_matrix = boost::make_shared<GlobalMatrixPCL>();
  global_matrix->setModel(global_model);

  UndistortionModel::Ptr model = boost::make_shared<UndistortionModel>();
  model->setLocalModel(local_model);
  model->setGlobalModel(global_model);

//  UndistortionPCL::Ptr undistortion = boost::make_shared<UndistortionPCL>();
//  undistortion->setModel(model);

  depth_sensor_->setUndistortionModel(model);

  publisher_ = boost::make_shared<Publisher>(node_handle_);

  test_ = boost::make_shared<CalibrationTest>();

  test_->setCheckerboards(cb_vec_);
  test_->setDepthSensor(depth_sensor_);
  test_->setColorSensor(color_sensor_);
  test_->setLocalModel(local_model);
  test_->setGlobalModel(global_model);
  test_->setPublisher(publisher_);
  test_->setDownSampleRatio(downsample_ratio_);

  return true;
}

void TestNode::checkerboardArrayCallback(const CheckerboardArray::ConstPtr & msg)
{
  checkerboard_array_msg_ = msg;
}

bool TestNode::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 TestNode::createCheckerboard(const CheckerboardMsg::ConstPtr & msg,
                                               int id)
{
  return createCheckerboard(*msg, id);
}

Checkerboard::Ptr TestNode::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;
}

void TestNode::spin()
{
  ros::Rate rate(10.0);

  int added = 0;

  /*std::fstream fs;
  fs.open("/tmp/points.txt", std::fstream::out);
  fs.close();*/

  ROS_INFO("Getting data...");
  for (int i = starting_index_; ros::ok() and i < starting_index_ + instances_; ++i)
  {

    std::stringstream image_file, cloud_file, depth_file;
    image_file << path_ << image_filename_ << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << "." << image_extension_;
    cloud_file << path_ << cloud_filename_ << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".pcd";
    depth_file << path_ << "depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";

    cv::Mat image = cv::imread(image_file.str());

    if (not image.data)
      continue;

    PCLCloud3::Ptr cloud;

    pcl::PCDReader pcd_reader;
    if (depth_type_ == KINECT1_DEPTH)
    {
      cloud = boost::make_shared<PCLCloud3>();

      if (pcd_reader.read(cloud_file.str(), *cloud) < 0)
      {
        ROS_WARN_STREAM(cloud_file.str() << " not valid!");
        continue;
      }

      /*cv::Mat depth = cv::imread(depth_file.str(), 2);
      cloud->width = depth.cols;
      cloud->height = depth.rows;
      cloud->points.resize(cloud->width * cloud->height);
      cloud->is_dense = false;

      for (int k = 0; k < cloud->width; ++k)
      {
        for (int j = 0; j < cloud->height; ++j)
        {
          if (depth.at<uint16_t>(j, k) > 0)
            cloud->at(k, j).z = depth.at<uint16_t>(j, k) / 1000.0f;
          else
            cloud->at(k, j).z = std::numeric_limits<float>::quiet_NaN();
        }
      }*/

    }

#ifdef HERRERA
    PCLCloud3::Ptr cloud_ = PCLCloud3::Ptr(new PCLCloud3(cloud->width, cloud->height));
    cloud_->header = cloud->header;
    cloud_->is_dense = cloud->is_dense;
    for (size_t p_i = 0; p_i < cloud->points.size(); ++p_i)
    {
      int row = p_i % cloud->height;
      int column = p_i / cloud->height;
      int index = row * cloud->width + column;
      cloud_->points[index].x = cloud->points[p_i].x;
      cloud_->points[index].y = cloud->points[p_i].y;
      cloud_->points[index].z = cloud->points[p_i].z;

    }
    cloud = cloud_;
#else
#  if (!defined(UNCALIBRATED)) || (defined(UNCALIBRATED) && defined(SKEW))
    //std::cout << cv::getOptimalNewCameraMatrix(color_sensor_->cameraModel()->intrinsicMatrix(), color_sensor_->cameraModel()->distortionCoeffs(),
    //		color_sensor_->cameraModel()->fullResolution(), 0) << std::endl;

    const Scalar & fx = depth_intrinsics_[0];
    const Scalar & fy = depth_intrinsics_[1];
    const Scalar & cx = depth_intrinsics_[2];
    const Scalar & cy = depth_intrinsics_[3];
#    ifdef SKEW
    const Scalar & s = depth_intrinsics_[4];
#    endif
    
    for (int k = 0; k < cloud->width; ++k)
    {
      for (int j = 0; j < cloud->height; ++j)
      {
#    ifdef SKEW
        // x = (z*(k*fy - cx*fy - j*s + cy*s))/(fx*fy)
        cloud->at(k, j).x = (k*fy - cx*fy - j*s + cy*s) * cloud->at(k, j).z / (fx*fy);
#    else
        cloud->at(k, j).x = (k - cx) * cloud->at(k, j).z / fx;
#    endif
        cloud->at(k, j).y = (j - cy) * cloud->at(k, j).z / fy;
      }
    }
#  endif
#endif


    PCLCloudRGB::Ptr cloud_rgb = test_->addData(image, cloud);
    ++added;

    std::stringstream cloud_rgb_file;
    cloud_rgb_file << path_ << cloud_filename_ << (i < 10 ? "000" : "00") << i << "_rgb.pcd";

    pcl::PCDWriter pcd_writer;
    if (depth_type_ == KINECT1_DEPTH)
    {
      if (pcd_writer.write(cloud_rgb_file.str(), *cloud_rgb) < 0)
      {
        ROS_WARN_STREAM(cloud_rgb_file.str() << " not valid!");
        continue;
      }
    }

    //rate.sleep();
  }
  ROS_INFO_STREAM("Added " << added << " images + point clouds.");
  if (not only_show_)
  {
//    test_->visualizeClouds();
//    test_->testPlanarityError();
//    test_->testCheckerboardError();
    test_->testCube();
  }


  rate = ros::Rate(1.0);

  while (ros::ok())
  {
    test_->publishData();
    rate.sleep();
  }

}

void TestNode::spin2()
{
  ros::Rate rate(10.0);

  ROS_INFO("Getting data...");
  for (int i = 1000; ros::ok() and i < 5000; ++i)
  {
    std::stringstream depth_file, depth_file_und;
    depth_file << path_ << "depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";
    depth_file_und << path_ << "und/depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";

    cv::Mat depth = cv::imread(depth_file.str(), 2);

    if (not depth.data)
      continue;

    for (int k = 0; k < depth.rows; ++k)
    {
      for (Size1 j = 0; j < depth.cols; ++j)
      {
        Scalar z = depth.at<uint16_t>(k, j) / Scalar(1000.0);
        if (z == 0)
          continue;

        depth_sensor_->undistortionModel()->localModel()->undistort(j, k, z);
        depth_sensor_->undistortionModel()->globalModel()->undistort(j, k, z);

        depth.at<uint16_t>(k, j) = cv::saturate_cast<uint16_t>(cvRound(z * 1000));
      }
    }

    cv::imwrite(depth_file_und.str(), depth);

  }
  ROS_INFO("OK");
}

void TestNode::spin3 ()
{
  ros::Rate rate(10.0);

  const Scalar & fx = depth_intrinsics_[0];
  const Scalar & fy = depth_intrinsics_[1];
  const Scalar & cx = depth_intrinsics_[2];
  const Scalar & cy = depth_intrinsics_[3];

  Transform t_original = Eigen::Affine3d::Identity() * Translation3(-0.025, 0.0, 0.0);
  Transform t = color_sensor_->pose().inverse();

  ROS_INFO("Getting data...");
  for (int i = 0; ros::ok() and i < 5000; ++i)
  {
    std::stringstream depth_file_2, depth_file, depth_file_und;
    depth_file << path_ << "depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";
    depth_file_2 << path_ << "alberto/depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";
    depth_file_und << path_ << "alberto/und/depth_" << (i < 10 ? "000" : (i < 100 ? "00" : (i < 1000 ? "0" : ""))) << i << ".png";

    cv::Mat depth = cv::imread(depth_file.str(), 2);

    if (not depth.data)
      continue;

    cv::Mat_<uint16_t> depth_2 = cv::Mat(depth.size(), 0);
    cv::Mat_<uint16_t> depth_und = cv::Mat(depth.size(), 0);

    for (int k = 0; k < depth.rows; ++k)
    {
      for (int j = 0; j < depth.cols; ++j)
      {
        Scalar z = depth.at<uint16_t>(k, j) / Scalar(1000.0);

        if (z > 0)
        {

          Point3 point_eigen;
          Point3 point_eigen_original;

          point_eigen.x() = (j - cx) * z / fx;
          point_eigen.y() = (k - cy) * z / fy;
          point_eigen.z() = z;

          point_eigen_original = t_original * point_eigen;
          Point2 point_image_original = color_sensor_->cameraModel()->project3dToPixel(point_eigen_original);

          uint16_t new_z_original = cv::saturate_cast<uint16_t>(cvRound(point_eigen_original.z() * 1000));
          int x = cvRound(point_image_original[0]);
          int y = cvRound(point_image_original[1]);

          if ((x >= 0 and x < depth.cols and y >= 0 and y < depth.rows) and
              (depth_2.at<uint16_t>(y, x) == 0 or new_z_original < depth_2.at<uint16_t>(y, x)))
            depth_2.at<uint16_t>(y, x) = new_z_original;


          depth_sensor_->undistortionModel()->localModel()->undistort(j, k, z);
          depth_sensor_->undistortionModel()->globalModel()->undistort(j, k, z);
          point_eigen.x() = (j - cx) * z / fx;
          point_eigen.y() = (k - cy) * z / fy;
          point_eigen.z() = z;

          point_eigen = t * point_eigen;
          Point2 point_image = color_sensor_->cameraModel()->project3dToPixel(point_eigen);

          uint16_t new_z = cv::saturate_cast<uint16_t>(cvRound(point_eigen.z() * 1000));
          x = cvRound(point_image[0]);
          y = cvRound(point_image[1]);

          if ((x >= 0 and x < depth.cols and y >= 0 and y < depth.rows) and
              (depth_und.at<uint16_t>(y, x) == 0 or new_z < depth_und.at<uint16_t>(y, x)))
            depth_und.at<uint16_t>(y, x) = new_z;

        }
      }
    }

    cv::imwrite(depth_file_und.str(), depth_und);
    cv::imwrite(depth_file_2.str(), depth_2);

  }
  ROS_INFO("OK");
}


} /* namespace calibration */

int main(int argc,
         char ** argv)
{
  ros::init(argc, argv, "test_calibration");
  ros::NodeHandle node_handle("~");

  try
  {
    calibration::TestNode test_node(node_handle);
    if (not test_node.initialize())
      return 0;
    test_node.spin();

    ros::spin();
  }
  catch (std::runtime_error & error)
  {
    ROS_FATAL("Test node error: %s", error.what());
    return 1;
  }

  return 0;
}


================================================
FILE: test/polynomial_undistortion_matrix_multifit_test.cpp
================================================
#include <calibration_common/base/math.h>
#include <rgbd_calibration/globals.h>
#include <kinect/depth/polynomial_matrix_fit.h>
#include <gtest/gtest.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

//using namespace calibration;

//typedef PolynomialUndistortionMatrixEigen<Polynomial<Types::Scalar, 2, 0> > UMap;
//typedef PolynomialUndistortionMatrixFitEigen<Polynomial<Types::Scalar, 2, 0> > UMapFit;

//TEST(PolynomialUndistortionMatrixFit, PolynomialUndistortionMatrixFit)
//{
//  UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);
//  SUCCEED();
//}
//
//TEST(PolynomialUndistortionMatrixFit, getIndex)
//{
//  UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);
//  size_t x, y;
//  u_map.getIndex(UMap::toSphericalCoordinates(Types::Point3(0.01, -0.01, 1.0)), x, y);
//  EXPECT_EQ(5, x);
//  EXPECT_EQ(5, y);
//}
//
//TEST(PolynomialUndistortionMatrixFit, updateDistortionMap)
//{
//  UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);
//
//  // applied distortion: z_d = c3 * z_p*z_p + c2 * z_p + c1
//  Types::Scalar c0 = 1.0;
//  Types::Scalar c1 = -2.0;
//  Types::Scalar c2 = 1.0;
//
//  // gaussian noise on the detected point, linear with respect to the z coordinate.
//  boost::mt19937 rnd_gen;
//  boost::normal_distribution<double> norm_dist(0.0, 1e-4);
//  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > normal_gen(rnd_gen, norm_dist);
//
//  // update ignore bins with less than 2*Dimension (6) points.
//  for (Types::Scalar z = 1.0; z <= 10.0; z++)
//    for (int n = 0; n < 10; n++)
//      u_map.addPoint(Types::Point3(0.0, 0.0, z), Types::Plane(-Eigen::Vector3d::UnitZ(), c2 * z * z + (c1 + normal_gen()) * z + c0));
//  u_map.update();
//
//  size_t x, y;
//  u_map.getIndex(UMap::toSphericalCoordinates(Types::Point3(0.0, 0.0, 1.0)), x, y);
//  EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[0], c0, 1e-2);
//  EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[1], c1, 1e-2);
//  EXPECT_NEAR(u_map.polynomialAt(x, y).coefficients()[2], c2, 1e-2);
//}
//
//TEST(PolynomialUndistortionMatrixFit, accumulatePoint)
//{
//  Types::Scalar c0 = 1.0;
//  Types::Scalar c1 = -2.0;
//  Types::Scalar c2 = 1.0;
//
//  boost::mt19937 rnd_gen;
//  boost::normal_distribution<double> norm_dist(0.0, 1e-4);
//  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > normal_gen(rnd_gen, norm_dist);
//
//  UMapFit u_map(10, 10, M_PI / 3.0, M_PI / 5.0);
//  UMapFit u_map2(10, 10, M_PI / 3.0, M_PI / 5.0);
//
//  for (double z = 1.0; z <= 10.0; z++)
//  {
//    for (int n = 0; n < 10; n++)
//    {
//      Types::Plane plane(-Eigen::Vector3d::UnitZ(), c2 * z * z + (c1 + normal_gen()) * z + c0);
//      u_map.addPoint(pcl::PointXYZ(0.0, 0.0, z), plane);
//      u_map2.accumulatePoint(pcl::PointXYZ(0.0, 0.0, z));
//      u_map2.addAccumulatedPoints(plane);
//    }
//  }
//  u_map.update();
//  u_map2.update();
//
//  EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[0], u_map2.polynomialAt(0, 0).coefficients()[0]);
//  EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[1], u_map2.polynomialAt(0, 0).coefficients()[1]);
//  EXPECT_EQ(u_map.polynomialAt(0, 0).coefficients()[2], u_map2.polynomialAt(0, 0).coefficients()[2]);
//}

int main(int argc,
         char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}