├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── kinect_tilt_calibration │ └── KinectTiltCalibration.hpp ├── launch ├── custom_rosconsole.conf ├── example.launch ├── realsense_d435.launch └── realsense_zr300.launch ├── package.xml └── src ├── KinectTiltCalibration.cpp └── kinect_tilt_calibration_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | 23 | # Eclipse 24 | .cproject 25 | .project 26 | .settings 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kinect_tilt_calibration) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 5 | 6 | # Set the build type. Options are: 7 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 8 | # Debug : w/ debug symbols, w/o optimization 9 | # Release : w/o debug symbols, w/ optimization 10 | # RelWithDebInfo : w/ debug symbols, w/ optimization 11 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 12 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 13 | 14 | ## Find catkin macros and libraries 15 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 16 | ## is used, also find other catkin packages 17 | find_package(catkin REQUIRED 18 | cmake_modules 19 | roscpp 20 | pcl_ros 21 | tf 22 | geometry_msgs 23 | kindr_ros 24 | ) 25 | 26 | ## System dependencies are found with CMake's conventions 27 | find_package(Eigen REQUIRED) 28 | 29 | # Attempt to find catkinized kindr 30 | find_package(kindr QUIET) 31 | if(NOT kindr_FOUND) 32 | # Attempt to find package-based kindr 33 | find_package(PkgConfig REQUIRED) 34 | pkg_check_modules(kindr kindr REQUIRED) 35 | endif() 36 | 37 | 38 | ################################### 39 | ## catkin specific configuration ## 40 | ################################### 41 | ## The catkin_package macro generates cmake config files for your package 42 | ## Declare things to be passed to dependent projects 43 | ## INCLUDE_DIRS: uncomment this if you package contains header files 44 | ## LIBRARIES: libraries you create in this project that dependent projects also need 45 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 46 | ## DEPENDS: system dependencies of this project that dependent projects also need 47 | catkin_package( 48 | INCLUDE_DIRS include 49 | # LIBRARIES kinect_tilt_calibration 50 | # CATKIN_DEPENDS other_catkin_pkg 51 | # DEPENDS system_lib 52 | ) 53 | 54 | ########### 55 | ## Build ## 56 | ########### 57 | 58 | ## Specify additional locations of header files 59 | ## Your package locations should be listed before other locations 60 | include_directories( 61 | include 62 | ${catkin_INCLUDE_DIRS} 63 | ${kindr_INCLUDE_DIRS} 64 | ) 65 | 66 | ## Declare a cpp library 67 | # add_library(kinect_tilt_calibration 68 | # src/${PROJECT_NAME}/kinect_tilt_calibration.cpp 69 | # ) 70 | 71 | ## Declare a cpp executable 72 | add_executable( 73 | ${PROJECT_NAME} 74 | src/kinect_tilt_calibration_node.cpp 75 | src/KinectTiltCalibration.cpp 76 | ) 77 | 78 | ## Add cmake target dependencies of the executable/library 79 | ## as an example, message headers may need to be generated before nodes 80 | # add_dependencies(kinect_tilt_calibration_node kinect_tilt_calibration_generate_messages_cpp) 81 | 82 | ## Specify libraries to link a library or executable target against 83 | target_link_libraries( 84 | ${PROJECT_NAME} 85 | ${catkin_LIBRARIES} 86 | ) 87 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Péter Fankhauser, Autonomous Systems Lab, ETH Zurich 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Kinect Tilt Calibration 2 | ====================== 3 | 4 | Overview 5 | --------------- 6 | 7 | This is a simple [ROS] node to calibrate the tilt angles (pitch and roll) of a Kinect-like depth sensor. It works by averaging a plane from the depth data and computing the rotation that aligns this plane with the world horizontal plane. 8 | 9 | The Kinect tilt calibration has been tested under ROS Indigo and Ubuntu 14.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 10 | 11 | **Author: Peter Fankhauser, pfankhauser@ethz.ch
12 | Affiliation: Autonomous Systems Lab, ETH Zurich** 13 | 14 | 15 | Installation 16 | ------------ 17 | 18 | ### Dependencies 19 | 20 | This software is built on the Robotic Operating System ([ROS]), which needs to be [installed](http://wiki.ros.org) first. Additionaly, the it depends on following software: 21 | 22 | - [Point Cloud Library (PCL)](http://pointclouds.org/), 23 | - [kindr](http://github.com/ethz-asl/kindr) (kinematics and dynamics library for robotics), 24 | - [Eigen](http://eigen.tuxfamily.org) (linear algebra library). 25 | 26 | 27 | ### Building 28 | 29 | In order to install the Kinect tilt calibration, clone the latest version from this repository into your catkin workspace and compile the package using ROS. 30 | 31 | cd catkin_workspace/src 32 | git clone https://github.com/ethz-asl/kinect_tilt_calibration.git 33 | cd ../ 34 | catkin_make 35 | 36 | 37 | Usage 38 | ------------ 39 | 40 | Place your robot on a flat floor in its nominal configuration and make sure that all the Kinect sensor captures is the floor. Copy the launch-file from `launch/example.launch` and adapt the parameters. 41 | 42 | roslaunch kinect_tilt_calibration your_launch_file.launch 43 | 44 | 45 | Nodes 46 | ------------ 47 | 48 | ### Node: kinect_tilt_calibration 49 | 50 | #### Subscribed Topics 51 | 52 | * **`/camera/depth/points`** ([sensor_msgs/PointCloud2]) 53 | 54 | The point cloud messages from the distance sensor. 55 | 56 | 57 | #### Published Topics 58 | 59 | * **`point_cloud_inliers`** ([sensor_msgs/PointCloud2]) 60 | 61 | The aggregated point cloud used for the plane estimation. 62 | 63 | 64 | #### Parameters 65 | 66 | * **`topic`** (string, default: "/camera/depth/points") 67 | 68 | The name of the point cloud messages from the distance sensor. 69 | 70 | * **`tilting_frame`** (string, default: "camera_link") 71 | 72 | Reference frame id of the frame for which the rotation should be estimated. This does not have to be the frame in which the point clouds are published. *Note: The parent from of the `tilting_frame` is assumed to be aligned with the world vertical axis.* 73 | 74 | * **`distance_threshold`** (double, default: 0.01) 75 | 76 | The distance threshold to determine plane segmentation [m]. 77 | 78 | * **`number_of_snapshots`** (int, default: 10) 79 | 80 | The number of point clouds to average for calibration 81 | 82 | 83 | Bugs & Feature Requests 84 | ------------ 85 | 86 | Please report bugs and request features using the [Issue Tracker](https://github.com/ethz-asl/kinect_tilt_calibration/issues). 87 | 88 | 89 | [ROS]: http://www.ros.org 90 | [rviz]: http://wiki.ros.org/rviz 91 | [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html 92 | -------------------------------------------------------------------------------- /include/kinect_tilt_calibration/KinectTiltCalibration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KinectTiltCalibration.hpp 3 | * 4 | * Created on: Nov 27, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #pragma once 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // PCL 18 | #include 19 | 20 | // Eigen 21 | #include 22 | 23 | namespace kinect_tilt_calibration { 24 | 25 | class KinectTiltCalibration 26 | { 27 | public: 28 | KinectTiltCalibration(ros::NodeHandle& nodeHandle); 29 | virtual ~KinectTiltCalibration(); 30 | 31 | typedef pcl::PointCloud PointCloud; 32 | 33 | /*! 34 | * Add point cloud to be considered in the calibration. 35 | * @param pointCloud the point cloud to be added. 36 | */ 37 | void addPointCloud(const PointCloud& pointCloud); 38 | 39 | /*! 40 | * Start calibration procedure. 41 | * @return true if successful, false otherwise. 42 | */ 43 | bool calibrate(); 44 | 45 | /*! 46 | * Computes the rotation to align the surface normal given in frameId with the vertical. 47 | * @param surfaceNormal the surface normal to align 48 | * @param frameId the frame id of the surface normal. 49 | * @return true if successful, false otherwise. 50 | */ 51 | bool computeRotation(const Eigen::Vector3d& surfaceNormal, const std::string& frameId); 52 | 53 | private: 54 | 55 | /*! 56 | * Reads and verifies the ROS parameters. 57 | * @return true if successful. 58 | */ 59 | bool readParameters(); 60 | 61 | /*! 62 | * Publish the inlier point cloud for visualization. 63 | * @param inlierIndices the indices of the inliers in the point cloud. 64 | */ 65 | void publishInliers(pcl::PointIndices::ConstPtr inlierIndices); 66 | 67 | /*! 68 | * Quits the node. 69 | */ 70 | void exit(); 71 | 72 | //! ROS nodehandle. 73 | ros::NodeHandle& nodeHandle_; 74 | 75 | //! Point cloud subscriber. 76 | ros::Subscriber pointCloudSubscriber_; 77 | 78 | //! Point cloud topic. 79 | std::string pointCloudTopic_; 80 | 81 | //! Point cloud publisher for inlier visualization. 82 | ros::Publisher pointCloudPublisher_; 83 | 84 | //! Tf listener. 85 | tf::TransformListener tfListener_; 86 | 87 | //! Number of point clouds to average for calibration. 88 | unsigned int maxSnapshots_; 89 | 90 | //! Current number of point clouds. 91 | unsigned int nSnapshots_; 92 | 93 | //! Aggregated point cloud. 94 | PointCloud::Ptr pointCloud_; 95 | 96 | //! Distance threshold to determine plane segmentation. 97 | double distanceThreshold_; 98 | 99 | //! Reference frame id of the frame for which the 100 | //! rotation should be estimated. This does not have to be 101 | //! the frame in which the point clouds are published. 102 | std::string tiltingFrameId_; 103 | }; 104 | 105 | } /* namespace kinect_tilt_calibration */ 106 | -------------------------------------------------------------------------------- /launch/custom_rosconsole.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.kinect_tilt_calibration=INFO -------------------------------------------------------------------------------- /launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/realsense_d435.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/realsense_zr300.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kinect_tilt_calibration 4 | 0.1.0 5 | Calibration tool to estimate the pitch and roll angle of a Kinect-type depth sensor. 6 | 7 | Peter Fankhauser 8 | MIT 9 | 10 | Peter Fankhauser 11 | 12 | catkin 13 | 14 | roscpp 15 | cmake_modules 16 | sensor_msgs 17 | pcl_ros 18 | tf 19 | geometry_msgs 20 | kindr_ros 21 | -------------------------------------------------------------------------------- /src/KinectTiltCalibration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KinectTiltCalibration.cpp 3 | * 4 | * Created on: Nov 27, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include "kinect_tilt_calibration/KinectTiltCalibration.hpp" 10 | 11 | // ROS 12 | #include 13 | 14 | // STD 15 | #include 16 | 17 | // PCL 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // Kindr 26 | #include 27 | #include 28 | 29 | using namespace std; 30 | 31 | namespace kinect_tilt_calibration { 32 | 33 | KinectTiltCalibration::KinectTiltCalibration(ros::NodeHandle& nodeHandle) 34 | : nodeHandle_(nodeHandle), 35 | pointCloud_(new PointCloud), 36 | nSnapshots_(0) 37 | { 38 | ROS_INFO("Kinect tilt calibration node started."); 39 | readParameters(); 40 | pointCloudSubscriber_ = nodeHandle_.subscribe(pointCloudTopic_, 1, &KinectTiltCalibration::addPointCloud, this); 41 | pointCloudPublisher_ = nodeHandle_.advertise("point_cloud_inliers", 1, true); 42 | } 43 | 44 | KinectTiltCalibration::~KinectTiltCalibration() {} 45 | 46 | bool KinectTiltCalibration::readParameters() 47 | { 48 | nodeHandle_.param("topic", pointCloudTopic_, string("/camera/depth/points")); 49 | nodeHandle_.param("distance_threshold", distanceThreshold_, 0.01); 50 | nodeHandle_.param("tilting_frame", tiltingFrameId_, string("camera_link")); 51 | 52 | int maxSnapshotsTemp; 53 | nodeHandle_.param("number_of_snapshots", maxSnapshotsTemp, 10); 54 | if (maxSnapshotsTemp < 0) { 55 | ROS_ERROR("'number_of_point_clouds' is smaller then 0!"); 56 | return false; 57 | } 58 | maxSnapshots_ = (unsigned int) maxSnapshotsTemp; 59 | 60 | return true; 61 | } 62 | 63 | void KinectTiltCalibration::addPointCloud(const PointCloud& pointCloud) 64 | { 65 | if (nSnapshots_ >= maxSnapshots_) return; 66 | ++nSnapshots_; 67 | *pointCloud_ += pointCloud; 68 | pointCloud_->header = pointCloud.header; 69 | ROS_INFO_STREAM("Added " << nSnapshots_ << " point clouds."); 70 | 71 | if (nSnapshots_ == maxSnapshots_) { 72 | calibrate(); 73 | exit(); 74 | } 75 | } 76 | 77 | bool KinectTiltCalibration::calibrate() 78 | { 79 | ROS_INFO_STREAM("Collected " << pointCloud_->width << " points."); 80 | 81 | // Find biggest plane. 82 | pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); 83 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices); 84 | pcl::SACSegmentation segmentation; 85 | segmentation.setOptimizeCoefficients(true); 86 | segmentation.setModelType(pcl::SACMODEL_PLANE); 87 | segmentation.setMethodType(pcl::SAC_RANSAC); 88 | segmentation.setDistanceThreshold(distanceThreshold_); 89 | segmentation.setInputCloud(pointCloud_); 90 | segmentation.segment(*inliers, *coefficients); 91 | 92 | if (inliers->indices.size() == 0) { 93 | ROS_ERROR("Could not estimate a planar model for the dataset."); 94 | return false; 95 | } 96 | 97 | ROS_INFO_STREAM("Calibrating with " << inliers->indices.size() << " inlier points."); 98 | Eigen::Vector3d normal(coefficients->values[0], coefficients->values[1], coefficients->values[2]); 99 | ROS_DEBUG_STREAM("Surface normal in sensor frame (" << pointCloud_->header.frame_id << "): " << normal.transpose()); 100 | if (!computeRotation(normal, pointCloud_->header.frame_id)) return false; 101 | 102 | ROS_INFO("Calibration done."); 103 | publishInliers(inliers); 104 | return true; 105 | } 106 | 107 | bool KinectTiltCalibration::computeRotation(const Eigen::Vector3d& surfaceNormal, const std::string& frameId) 108 | { 109 | // Get surface normal in tilting frame. 110 | tf::StampedTransform transform; 111 | try { 112 | tfListener_.lookupTransform(tiltingFrameId_, frameId, ros::Time(0), transform); 113 | } catch (tf2::TransformException& ex) { 114 | ROS_ERROR("%s", ex.what()); 115 | return false; 116 | } 117 | kindr::HomTransformQuatD pose; 118 | kindr_ros::convertFromRosTf(transform, pose); 119 | ROS_DEBUG_STREAM("Pose of sensor frame in tilting frame: " << endl << pose << endl); 120 | Eigen::Vector3d surfaceNormalInTiltingFrame = pose.getRotation().rotate(surfaceNormal); 121 | if (surfaceNormalInTiltingFrame.z() < 0.0) surfaceNormalInTiltingFrame = -surfaceNormalInTiltingFrame; 122 | ROS_DEBUG_STREAM("Surface normal in tilting frame (" << tiltingFrameId_ << "): " << surfaceNormalInTiltingFrame.transpose()); 123 | 124 | // Compute calibration angles. 125 | Eigen::Vector3d reference = Eigen::Vector3d::UnitZ(); 126 | kindr::RotationQuaternionPD rotation; 127 | rotation.setFromVectors(surfaceNormalInTiltingFrame, reference); 128 | 129 | cout << "===============================" << endl; 130 | cout << "Quaternion (qx, qy, qz, qw): " << rotation.x() << ", " << rotation.y() << ", " << rotation.z() << ", " << rotation.w() << endl; 131 | kindr::EulerAnglesYprPD euler(rotation); 132 | Eigen::Vector3d eulerVector = euler.getUnique().toImplementation() / M_PI * 180.0; 133 | cout << "Pitch: " << eulerVector(1) << " deg, Roll: " << eulerVector(2) << " deg" << endl; // Yaw should be 0 up to numerical errors. 134 | cout << "===============================" << endl; 135 | 136 | return true; 137 | } 138 | 139 | void KinectTiltCalibration::publishInliers(pcl::PointIndices::ConstPtr inlierIndices) 140 | { 141 | if (pointCloudPublisher_.getNumSubscribers() < 1) return; 142 | PointCloud inlierPointCloud; 143 | pcl::ExtractIndices extractIndicesFilter(true); 144 | extractIndicesFilter.setInputCloud(pointCloud_); 145 | extractIndicesFilter.setIndices(inlierIndices); 146 | extractIndicesFilter.filter(inlierPointCloud); 147 | ROS_INFO("Publishing inlier point cloud."); 148 | pointCloudPublisher_.publish(inlierPointCloud); 149 | } 150 | 151 | void KinectTiltCalibration::exit() 152 | { 153 | if (pointCloudPublisher_.getNumSubscribers() > 0) { 154 | ros::Duration duration(1.0); 155 | duration.sleep(); 156 | } 157 | ros::requestShutdown(); 158 | } 159 | 160 | } /* namespace kinect_tilt_calibration */ 161 | -------------------------------------------------------------------------------- /src/kinect_tilt_calibration_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * kinect_tilt_calibration_node.cpp 3 | * 4 | * Created on: Nov 27, 2013 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, Autonomous Systems Lab 7 | */ 8 | 9 | #include 10 | #include "kinect_tilt_calibration/KinectTiltCalibration.hpp" 11 | 12 | int main(int argc, char** argv) 13 | { 14 | ros::init(argc, argv, "kinect_tilt_calibration"); 15 | 16 | ros::NodeHandle nodeHandle("~"); 17 | 18 | kinect_tilt_calibration::KinectTiltCalibration kinectTiltCalibration(nodeHandle); 19 | 20 | ros::spin(); 21 | return 0; 22 | } 23 | --------------------------------------------------------------------------------