diff --git a/.gitignore b/.gitignore index b7fd55b..14a6436 100644 --- a/.gitignore +++ b/.gitignore @@ -23,4 +23,5 @@ srv_gen/ launch/calibrated_tf* scripts/ -src/mono_pattern.cpp +.idea/ +.vscode/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c301d9..9db2aac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(velo2cam_calibration) -SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -w ") +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -w ") ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS @@ -18,19 +18,16 @@ find_package(catkin REQUIRED COMPONENTS tf_conversions dynamic_reconfigure cmake_modules - # geometry_msgs - # tf2 - # tf2_ros - # tf2_sensor_msgs ) find_package(TinyXML REQUIRED) -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV REQUIRED) find_package(PCL 1.7 REQUIRED) generate_dynamic_reconfigure_options( - cfg/Laser.cfg - cfg/Camera.cfg + cfg/Lidar.cfg + cfg/Stereo.cfg + cfg/Monocular.cfg cfg/Plane.cfg ) @@ -78,18 +75,16 @@ add_definitions( ) ## Declare cpp executables -add_executable(laser_pattern src/laser_pattern.cpp) +add_executable(lidar_pattern src/lidar_pattern.cpp) add_executable(stereo_pattern src/stereo_pattern.cpp) +add_executable(mono_qr_pattern src/mono_qr_pattern.cpp) add_executable(velo2cam_calibration src/velo2cam_calibration.cpp) -add_executable(pcl_coloring src/pcl_coloring.cpp) -add_executable(pcl_projection src/pcl_projection.cpp) -add_executable(levinson src/levinson.cpp) add_executable(v2c_plane_segmentation src/plane.cpp) add_executable(v2c_disp_masker src/disp_masker.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -add_dependencies(laser_pattern +add_dependencies(lidar_pattern ${catkin_EXPORTED_TARGETS} velo2cam_calibration_generate_messages_cpp ${PROJECT_NAME}_gencfg @@ -99,13 +94,17 @@ add_dependencies(stereo_pattern velo2cam_calibration_generate_messages_cpp ${PROJECT_NAME}_gencfg ) +add_dependencies(mono_qr_pattern + ${catkin_EXPORTED_TARGETS} + velo2cam_calibration_generate_messages_cpp + ${PROJECT_NAME}_gencfg +) add_dependencies( velo2cam_calibration ${catkin_EXPORTED_TARGETS} velo2cam_calibration_generate_messages_cpp ${PROJECT_NAME}_gencfg ) -add_dependencies(levinson ${catkin_EXPORTED_TARGETS}) add_dependencies(v2c_plane_segmentation ${PROJECT_NAME}_gencfg) ## Specify libraries to link a library or executable target against @@ -114,31 +113,21 @@ target_link_libraries(stereo_pattern ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) -target_link_libraries(laser_pattern - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} -) -target_link_libraries(velo2cam_calibration - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} - ${PCL_LIBRARIES} - ${TinyXML_LIBRARIES} -) -target_link_libraries(pcl_coloring +target_link_libraries(lidar_pattern ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) -target_link_libraries(pcl_projection +target_link_libraries(mono_qr_pattern ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) -target_link_libraries(levinson +target_link_libraries(velo2cam_calibration ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} + ${TinyXML_LIBRARIES} ) target_link_libraries(v2c_plane_segmentation ${catkin_LIBRARIES} diff --git a/HOWTO.md b/HOWTO.md new file mode 100644 index 0000000..db4a353 --- /dev/null +++ b/HOWTO.md @@ -0,0 +1,96 @@ +# User guide: How to calibrate your sensors +*Velo2cam_calibration* is a tool to obtain the extrinsic parameters expressing the relative pose of a pair of sensors. Each of these sensors can be a monocular camera, a stereo camera, or a lidar scanner. + +# Setup # +First, you must identify the topics delivered by the drivers of the sensors to be calibrated. In particular, velo2cam_calibration expects the following inputs: +* Monocular: a rectified image ([sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) and the corresponding meta-information topic ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)). Both topics are expected to be referred to the same *frame_id*. +* Stereo: a pair of rectified images (2x [sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) and the corresponding meta-information topic ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)). All the topics are expected to be referred to the same *frame_id*. +* LiDAR: a 3D point cloud ([sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html)). + +To start the calibration procedure, the reference point extraction nodes must be run. To that end, you can use the launch files included in this same repository. Arguments are available to pass the topic and frame names to the nodes. A different launch file must be used for each sensor, and it will be different depending on its modality: + +* Monocular: *mono_pattern.launch*. Arguments to be set: + * *camera_name*: camera namespace in which the image topic and the meta-information topics exist. + * *image_topic*: name of the image topic excluding the camera namespace (e.g., *image_rect_color*). + * *frame_name*: *frame_id* to which camera topics are referred to. +* Stereo: *stereo_pattern.launch*. Arguments to be set: + * *camera_name*: camera namespace. Images of the stereo pair and meta-information topics should exist under */left* and */right*. + * *image_topic*: name of the image topics under their respective namespaces (e.g., *image_rect_color*). + * *frame_name*: *frame_id* of the left image. +* LiDAR: *lidar_pattern.launch*. Arguments to be set: + * *cloud_topic*: LiDAR point cloud topic. + +In all cases, if the two sensors to be calibrated belong to the same modality, an additional argument *sensor_id* must be set so that one of the devices is given *sensor_id=0* and the other, *sensor_id=1*. + +Apart from this, a third launch file must be run to perform the registration step: +* Registration: *registration.launch*. Arguments to be set: + * *sensor1_type* and *sensor2_type*: modalities of the sensors to be calibrated, to be chosen from *mono*, *stereo*, and *lidar*. + * *sensor1_id* and *sensor2_id*: to be set to 0 and 1, respectively, only if the two devices to be calibrated use the same modality. + +## Examples ## +For instance, if you want to calibrate a monocular camera delivering the topics */blackflys/image_rect_color* (rectified_image) and */blackflys/camera_info* (meta-information), both referred to the *blackflys* *frame_id*, and a LiDAR scanner providing a */hdl64_points* cloud, you should run the following commands in three separate terminals: + +```roslaunch velo2cam_calibration mono_pattern.launch camera_name:=/blackflys image_topic:=image_rect_color frame_name:=blackflys``` + +```roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/hdl64_points``` + +```roslaunch velo2cam_calibration registration.launch sensor1_type:=mono sensor2_type:=lidar``` + + +On the other hand, if a stereo camera is involved, calibration can be performed using the monocular procedure on the left image of the pair. However, there also exists the possibility to use the specific stereo-based reference point extraction. If the stereo camera is publishing images in the */bumblebee_xb3/left/image_rect_color* and */bumblebee_xb3/left/image_rect_color* topics (with *frame_id* *bumblebee_xb3*), the launch files to use are: + +```roslaunch velo2cam_calibration stereo_pattern.launch camera_name:=/bumblebee_xb3 image_topic:=image_rect_color frame_name:=bumblebee_xb3``` + +```roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/hdl64_points``` + +```roslaunch velo2cam_calibration registration.launch sensor1_type:=stereo sensor2_type:=lidar``` + +Finally, if the sensors that will be calibrated are two LiDAR scanners, publishing */hdl64_points* and */hdl32_points* clouds, the *sensor_id* arguments must be set appropriately: + +```roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/hdl64_points sensor_id:=0``` + +```roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/hdl32_points sensor_id:=1``` + +```roslaunch velo2cam_calibration registration.launch sensor1_type:=lidar sensor2_type:=lidar sensor1_id:=0 sensor2_id:=1``` + +# Warm-up stage # +The instructions to perform the calibration will appear on the terminal where the registration launch file has been run. In the beginning, there is a warm-up stage where the user must check that the calibration pattern is correctly represented in the data from both sensors. The procedure takes place sequentially (one sensor after the other), and there are two tasks to be performed: one, to make sure that the pattern is within the field of view of the sensors; and two, to adjust the passthrough filters intended to prevent outliers in range data. Any sensor data visualization tool can be used at this point, although *rviz* is encouraged. Passthrough filters can be modified through *dynamic_reconfigure*. In a typical use case, the following GUIs can be launched in parallel: + +```rviz``` + +```rosrun rqt_reconfigure rqt_reconfigure``` + +*rviz* will be used to visualize the images and/or point clouds, and *rqt_reconfigure* to modify the filter parameters on the fly. Note that the registration node only will let the user advance when the pattern can be identified in the current sensor. If the calibration pattern needs to be moved, the procedure should be reset so that the adequacy of the new location is rechecked. + +For monocular devices, the only relevant data is the image, where the pattern should be fully visible. For range data, the checklist depends on the modality. + +## Stereo ## +The four pattern circles must be well visible in */z_filtered_cloud*. Otherwise, please modify the limits of the passthrough filter of the *full_pass_through_z* node in rqt_reconfigure (parameters *filter_limit_min* and *filter_limit_max*) so that they represent the area where the pattern is located. Then, apply the same values to the *edges_pass_through_z* node. + +As an example, please check the following image, where the default passthrough values are not suitable: + +![gazebo screenshot](screenshots/stereo_filters_1.png) + +After the *filter_limit_max* variable has been conveniently enlarged to 4 meters, the pattern becomes visible: + +![gazebo screenshot](screenshots/stereo_filters_2.png) + +## LiDAR ## +The *lidar_pattern_/zyx_filtered* cloud should represent only the calibration pattern and the points corresponding to those lidar rays that go through the holes. The cloud can be filtered through the parameters *filter_limit_min* and *filter_limit_max* of the *pass_through_x_velo_*, *pass_through_y_velo_*, and *pass_through_z_velo_* nodes. + +On the other hand, *lidar_pattern_/range_filtered_cloud* must contain only the calibration pattern. To that end, a radial passthrough filter is available in the *lidar_pattern_* node, tunable through the *passthrough_radius_min* and *passthrough_radius_max* parameters. + +Please check the following example, where the *lidar_pattern_/zyx_filtered* cloud is depicted in white and *lidar_pattern_/range_filtered_cloud*, in red. The initial parameters yield the following clouds: + +![gazebo screenshot](screenshots/lidar_filters_1.png) + +Ideally, the parameters should be set so that the clouds are close to the following: + +![gazebo screenshot](screenshots/lidar_filters_2.png) + +Note, however, that the filters do not need an exact tuning as long as the areas of interest (i.e., the pattern and the surface behind it) are well defined in the LiDAR clouds. + +## Calibration ## +When the pattern is visible in all the modalities, and the passthrough filters have been adjusted, the user can start the calibration. It is important to make sure that the scene is static during the whole process, which will require 30 frames where the calibration pattern is correctly identified. Some errors might be shown in the terminals where the reference point extraction launch files were run; it is safe to ignore them as long as the iteration count provided by the registration node increases. At the end of the calibration, the program will ask the user if a new pattern pose is needed. It is recommended to repeat the procedure at least three times, each with a different position and orientation of the calibration pattern. Each new iteration starts with the same warm-up stage described above, where the passthrough filters should be properly adjusted to the new pattern location. + +Calibration can be finished after any iteration. The final result will be immediately displayed, and a calibrated_tf.launch file will be created in the *launch* folder within the *velo2cam_calibration* package path, containing the *static_transform_publisher* able to broadcast the correct *tf* transformation. \ No newline at end of file diff --git a/README.md b/README.md index 78761a2..e8aeb5d 100644 --- a/README.md +++ b/README.md @@ -1,92 +1,34 @@ # velo2cam_calibration [![Build Status](http://build.ros.org/job/Kdev__velo2cam_calibration__ubuntu_xenial_amd64/badge/icon)](http://build.ros.org/job/Kdev__velo2cam_calibration__ubuntu_xenial_amd64/) -The *velo2cam_calibration* software implements an Automatic Calibration algorithm for Lidar-Stereo camera setups \[1\]. This software is provided as a ROS package. +The *velo2cam_calibration* software implements a state-of-the-art automatic calibration algorithm for pair of sensors composed of LiDAR and camera devices in any possible combination \[1\]. This software is provided as a ROS package. Package developed at [Intelligent Systems Laboratory](http://www.uc3m.es/islab), Universidad Carlos III de Madrid. -![gazebo screenshot](screenshots/velo2cam_calibration_setup.png) +![real results](screenshots/real_results.png) -# Nodes # -## stereo_pattern ## -### Subscribed Topics ### -*cloud2* ([sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html)) +## Setup ## +To install this ROS package: +1. Clone the repository into your *catkin_ws/src/* folder. +2. Install run dependencies: ```sudo apt-get install ros--opencv-apps``` +3. Build your workspace [as usual](http://wiki.ros.org/ROS/Tutorials/BuildingPackages). -    Stereo camera point cloud containing points belonging to edges in the left image +## Usage ## +See [HOWTO.md](HOWTO.md) for detailed instructions on how to use this software. -*cam_plane_coeffs* ([pcl_msgs::ModelCoefficients](http://docs.ros.org/api/pcl_msgs/html/msg/ModelCoefficients.html)) +To test the algorithm in a virtual environment, you can launch any of the calibration scenarios included in our [Gazebo validation suite](https://github.com/beltransen/velo2cam_gazebo). -    Coefficients of the calibration target plane model -### Published Topics ### -*/stereo_pattern/centers_cloud* ([velo2cam_calibration::ClusterCentroids](http://docs.ros.org/kinetic/api/velo2cam_calibration/html/msg/ClusterCentroids.html)) +## Calibration target ## +The following picture shows a possible embodiment of the proposed calibration target used by this algorithm and its corresponding dimensional drawing. -    Target circles centers obtained from stereo camera data +![calibration target](screenshots/calibration_target_real_scheme_journal.png) - -## laser_pattern ## -### Subscribed Topics ### -*cloud1* ([sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html)) +**Note:** Other size may be used for convenience. If so, please configure node parameters accordingly. -    LIDAR pointcloud -### Published Topics ### -*/laser_pattern/centers_cloud* ([velo2cam_calibration::ClusterCentroids](http://docs.ros.org/kinetic/api/velo2cam_calibration/html/msg/ClusterCentroids.html)) +## Citation ## +If you use this work in your research, please consider citing the following paper: -    Target circles centers obtained from LIDAR data - -## velo2cam_calibration ## -### Subscribed Topics ### -*~cloud1* ([velo2cam_calibration::ClusterCentroids](http://docs.ros.org/kinetic/api/velo2cam_calibration/html/msg/ClusterCentroids.html)) +[1] Beltrán, J., Guindel, C., and García, F. (2021). [Automatic Extrinsic Calibration Method for LiDAR and Camera Sensor Setups](https://arxiv.org/abs/2101.04431). arXiv:2101.04431 [cs.CV]. Submitted to IEEE Transactions on Intelligent Transportation Systems. -    Target circles centers obtained from LIDAR data +A previous version of this tool is available [here](https://github.com/beltransen/velo2cam_calibration/tree/v1.0) and was described on this paper: -*~cloud2* ([velo2cam_calibration::ClusterCentroids](http://docs.ros.org/kinetic/api/velo2cam_calibration/html/msg/ClusterCentroids.html)) - -    Target circles centers obtained from stereo camera data - -*~cloud3* ([sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html)) - -    Original LIDAR pointcloud -### Published Topics ### -TF containing the transformation between both sensors ([see TF info in ROS Wiki](http://wiki.ros.org/tf)) - -The node broadcasts the TF transformation between *velodyne* and *stereo* frames. -The fixed transformation between *stereo_camera* and *stereo* is published by a static broadcaster in *stereo_pattern.launch*. -The picture below shows the three coordinate frames: - -![gazebo screenshot](screenshots/coordinates_frames.png) - -**Note**: Additionally, a .launch file called *calibrated_tf.launch* containing the proper TF broadcasters is created in the */launch* folder of the *velo2cam_calibration* package so -you can use the calibration just executing: - -```roslaunch velo2cam_calibration calibrated_tf.launch``` - -### Parameters ### -As described in the paper, different parameters can be selected. The ones that you will usually need are set in the launch files. For convenience, two sets of launch files are provided, with different parameters intended for the Gazebo environment, on the one hand, or the real wood pattern, on the other hand. The latter have *real_* as a prefix. - -# Usage # -Some sample .launch files are provided in this package. The simplest way to launch the algorithm is by running the three main ROS nodes as follows: - -```roslaunch velo2cam_calibration laser_pattern.launch``` - -```roslaunch velo2cam_calibration stereo_pattern.launch``` - -```roslaunch velo2cam_calibration velo2cam_calibration.launch``` - -We also provide a launch file containing the three launch files above: - -```roslaunch velo2cam_calibration full_calibration.launch``` - -In order to test the algorithm, you can download an example .bag file [here](https://www.dropbox.com/s/fdvew31azdp9cbg/1_trans_0.bag?dl=1). - -**Note**: The .bag file above was generated using a simulator environment in Gazebo which is provided [here](https://github.com/beltransen/velo2cam_gazebo). We use the simulator in order to test the algorithm with a proper ground truth. - -# Calibration target details # -The following scheme shows the real size of the calibration target used by this algorithm. Measurements are given in centimeters (cm). - -![gazebo screenshot](screenshots/calibration_target_scheme.png) - -**Note:** Other size may be used for convenience. If so, please configure nodes parameters accordingly. - -# Citation # -\[1\] Guindel, C., Beltrán, J., Martín, D. and García, F. (2017). Automatic Extrinsic Calibration for Lidar-Stereo Vehicle Sensor Setups. *IEEE International Conference on Intelligent Transportation Systems (ITSC), 674–679*. - -Pre-print available [here](https://arxiv.org/abs/1705.04085). +[2] Guindel, C., Beltrán, J., Martín, D., and García, F. (2017). [Automatic Extrinsic Calibration for Lidar-Stereo Vehicle Sensor Setup](https://arxiv.org/abs/1705.04085). *IEEE International Conference on Intelligent Transportation Systems (ITSC), 674–679*. \ No newline at end of file diff --git a/cfg/Laser.cfg b/cfg/Lidar.cfg similarity index 86% rename from cfg/Laser.cfg rename to cfg/Lidar.cfg index c5ddfe0..dba9f92 100755 --- a/cfg/Laser.cfg +++ b/cfg/Lidar.cfg @@ -10,11 +10,11 @@ gen = ParameterGenerator() gen.add("x", double_t, 0, "x coord", 0, 0, 1) gen.add("y", double_t, 0, "y coord", 0, 0, 1) gen.add("z", double_t, 0, "z coord", 1, 0, 1) -gen.add("angle_threshold", double_t, 0, "Angle threshold for plane segmentation", 0.35, 0, pi/2) +gen.add("angle_threshold", double_t, 0, "Angle threshold for plane segmentation", 0.55, 0, pi/2) gen.add("circle_radius", double_t, 0, "Radius of pattern's circles", 0.12, 0, 1) gen.add("passthrough_radius_min", double_t, 0, "Min radius for passthrough", 1.0, 0, 10) gen.add("passthrough_radius_max", double_t, 0, "Max radius for passthrough", 6.0, 0, 10) gen.add("centroid_distance_min", double_t, 0, "Min distance to the centroid", 0.15, 0.0, 1.0) -gen.add("centroid_distance_max", double_t, 0, "Max distance to the centroid", 0.25, 0.0, 1.0) +gen.add("centroid_distance_max", double_t, 0, "Max distance to the centroid", 0.8, 0.0, 1.0) -exit(gen.generate(PACKAGE, "velo2cam_calibration", "Laser")) +exit(gen.generate(PACKAGE, "velo2cam_calibration", "Lidar")) diff --git a/cfg/Monocular.cfg b/cfg/Monocular.cfg new file mode 100755 index 0000000..1b25f41 --- /dev/null +++ b/cfg/Monocular.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python +PACKAGE = "velo2cam_calibration" + +from math import pi + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("marker_size", double_t, 0, "Size of the marker (m)", 0.2, 0.1, 0.5) +gen.add("delta_width_qr_center", double_t, 0, "width increment from target center to qr center (m)", 0.55, 0, 1) +gen.add("delta_height_qr_center", double_t, 0, "height increment from target center to qr center (m)", 0.35, 0, 1) + +exit(gen.generate(PACKAGE, "velo2cam_calibration", "Monocular")) diff --git a/cfg/Camera.cfg b/cfg/Stereo.cfg similarity index 62% rename from cfg/Camera.cfg rename to cfg/Stereo.cfg index cd85b19..53cc128 100755 --- a/cfg/Camera.cfg +++ b/cfg/Stereo.cfg @@ -6,6 +6,5 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("circle_threshold", double_t, 0, "Cam circle threshold", 0.01, 0, 5) -gen.add("line_threshold", double_t, 0, "Cam line threshold", 0.02, 0, 5) -exit(gen.generate(PACKAGE, "velo2cam_calibration", "Camera")) +exit(gen.generate(PACKAGE, "velo2cam_calibration", "Stereo")) diff --git a/include/velo2cam_utils.h b/include/velo2cam_utils.h index bea2b78..5943c50 100644 --- a/include/velo2cam_utils.h +++ b/include/velo2cam_utils.h @@ -1,6 +1,7 @@ /* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel This file is part of velo2cam_calibration. @@ -18,208 +19,367 @@ along with velo2cam_calibration. If not, see . */ +/* + velo2cam_utils: Helper functions +*/ + #ifndef velo2cam_utils_H #define velo2cam_utils_H #define PCL_NO_PRECOMPILE #define DEBUG 0 -#include -#include -#include -#include -#include -#include +#include #include -#include -#include -#include -using namespace std; +#define TARGET_NUM_CIRCLES 4 +#define TARGET_RADIUS 0.12 +#define GEOMETRY_TOLERANCE 0.06 -static const int RINGS_COUNT = 16; // TODO AS FUNCTION PARAM +using namespace std; namespace Velodyne { - struct Point - { - PCL_ADD_POINT4D; // quad-word XYZ - float intensity; ///< laser intensity reading - uint16_t ring; ///< laser ring number - float range; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment - }EIGEN_ALIGN16; - - void addRange(pcl::PointCloud & pc){ - for (pcl::PointCloud::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++) - { - pt->range = sqrt(pt->x * pt->x + pt->y * pt->y + pt->z * pt->z); - } +struct Point { + PCL_ADD_POINT4D; // quad-word XYZ + float intensity; ///< laser intensity reading + std::uint16_t ring; ///< laser ring number + float range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment +} EIGEN_ALIGN16; + +void addRange(pcl::PointCloud &pc) { + for (pcl::PointCloud::iterator pt = pc.points.begin(); + pt < pc.points.end(); pt++) { + pt->range = sqrt(pt->x * pt->x + pt->y * pt->y + pt->z * pt->z); } +} - vector > getRings(pcl::PointCloud & pc) - { - vector > rings(RINGS_COUNT); - for (pcl::PointCloud::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++) - { - rings[pt->ring].push_back(&(*pt)); - } - return rings; +vector> getRings(pcl::PointCloud &pc, + int rings_count) { + vector> rings(rings_count); + for (pcl::PointCloud::iterator pt = pc.points.begin(); + pt < pc.points.end(); pt++) { + rings[pt->ring].push_back(&(*pt)); } + return rings; +} - // all intensities to range min-max - void normalizeIntensity(pcl::PointCloud & pc, float minv, float maxv) - { - float min_found = INFINITY; - float max_found = -INFINITY; +// all intensities to range min-max +void normalizeIntensity(pcl::PointCloud &pc, float minv, float maxv) { + float min_found = INFINITY; + float max_found = -INFINITY; - for (pcl::PointCloud::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++) - { - max_found = max(max_found, pt->intensity); - min_found = min(min_found, pt->intensity); - } + for (pcl::PointCloud::iterator pt = pc.points.begin(); + pt < pc.points.end(); pt++) { + max_found = max(max_found, pt->intensity); + min_found = min(min_found, pt->intensity); + } - for (pcl::PointCloud::iterator pt = pc.points.begin(); pt < pc.points.end(); pt++) - { - pt->intensity = (pt->intensity - min_found) / (max_found - min_found) * (maxv - minv) + minv; - } + for (pcl::PointCloud::iterator pt = pc.points.begin(); + pt < pc.points.end(); pt++) { + pt->intensity = + (pt->intensity - min_found) / (max_found - min_found) * (maxv - minv) + + minv; } } +} // namespace Velodyne + +POINT_CLOUD_REGISTER_POINT_STRUCT(Velodyne::Point, + (float, x, x)(float, y, y)(float, z, z)( + float, intensity, + intensity)(std::uint16_t, ring, + ring)(float, range, range)); + +void sortPatternCenters(pcl::PointCloud::Ptr pc, + vector &v) { + // 0 -- 1 + // | | + // 3 -- 2 + + if (v.empty()) { + v.resize(4); + } -POINT_CLOUD_REGISTER_POINT_STRUCT( - Velodyne::Point, (float, x, x) (float, y, y) (float, z, z) - (float, intensity, intensity) (uint16_t, ring, ring) (float, range, range)); - -void sortPatternCentersXY(pcl::PointCloud::Ptr pc, vector &v){ - double avg_x = 0, avg_y = 0; - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - avg_x += (*it).x; - avg_y += (*it).y; - } - - pcl::PointXYZ center; - center.x = avg_x/4.; - center.y = avg_y/4.; - - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - double x_dif = (*it).x - center.x; - double y_dif = (*it).y - center.y; - - if(x_dif < 0 && y_dif < 0){ - v[0] = (*it); - }else if(x_dif > 0 && y_dif < 0){ - v[1] = (*it); - }else if(x_dif > 0 && y_dif > 0){ - v[2] = (*it); - }else{ - v[3] = (*it); + // Transform points to polar coordinates + pcl::PointCloud::Ptr spherical_centers( + new pcl::PointCloud()); + int top_pt = 0; + int index = 0; // Auxiliar index to be used inside loop + for (pcl::PointCloud::iterator pt = pc->points.begin(); + pt < pc->points.end(); pt++, index++) { + pcl::PointXYZ spherical_center; + spherical_center.x = atan2(pt->y, pt->x); // Horizontal + spherical_center.y = + atan2(sqrt(pt->x * pt->x + pt->y * pt->y), pt->z); // Vertical + spherical_center.z = + sqrt(pt->x * pt->x + pt->y * pt->y + pt->z * pt->z); // Range + spherical_centers->push_back(spherical_center); + + if (spherical_center.y < spherical_centers->points[top_pt].y) { + top_pt = index; } } -} -void sortPatternCentersYZ(pcl::PointCloud::Ptr pc, vector &v){ - double avg_y = 0, avg_z = 0; - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - avg_y += (*it).y; - avg_z += (*it).z; - } - - pcl::PointXYZ center; - center.y = avg_y/4.; - center.z = avg_z/4.; - - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - double y_dif = (*it).y - center.y; - double z_dif = (*it).z - center.z; - - if(y_dif > 0 && z_dif > 0){ - v[0] = (*it); - }else if(y_dif < 0 && z_dif > 0){ - v[1] = (*it); - }else if(y_dif < 0 && z_dif < 0){ - v[2] = (*it); - }else{ - v[3] = (*it); + // Compute distances from top-most center to rest of points + vector distances; + for (int i = 0; i < 4; i++) { + pcl::PointXYZ pt = pc->points[i]; + pcl::PointXYZ upper_pt = pc->points[top_pt]; + distances.push_back(sqrt(pow(pt.x - upper_pt.x, 2) + + pow(pt.y - upper_pt.y, 2) + + pow(pt.z - upper_pt.z, 2))); + } + + // Get indices of closest and furthest points + int min_dist = (top_pt + 1) % 4, max_dist = top_pt; + for (int i = 0; i < 4; i++) { + if (i == top_pt) continue; + if (distances[i] > distances[max_dist]) { + max_dist = i; + } + if (distances[i] < distances[min_dist]) { + min_dist = i; } } -} -void colourCenters(const pcl::PointCloud::Ptr pc, pcl::PointCloud::Ptr coloured){ - double avg_x = 0, avg_y = 0; - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - avg_x += (*it).x; - avg_y += (*it).y; + // Second highest point shoud be the one whose distance is the median value + int top_pt2 = 6 - (top_pt + max_dist + min_dist); // 0 + 1 + 2 + 3 = 6 + + // Order upper row centers + int lefttop_pt = top_pt; + int righttop_pt = top_pt2; + + if (spherical_centers->points[top_pt].x < + spherical_centers->points[top_pt2].x) { + int aux = lefttop_pt; + lefttop_pt = righttop_pt; + righttop_pt = aux; } - pcl::PointXYZ center; - center.x = avg_x/4.; - center.y = avg_y/4.; + // Swap indices if target is located in the pi,-pi discontinuity + double angle_diff = spherical_centers->points[lefttop_pt].x - + spherical_centers->points[righttop_pt].x; + if (angle_diff > M_PI - spherical_centers->points[lefttop_pt].x) { + int aux = lefttop_pt; + lefttop_pt = righttop_pt; + righttop_pt = aux; + } + + // Define bottom row centers using lefttop == top_pt as hypothesis + int leftbottom_pt = min_dist; + int rightbottom_pt = max_dist; - for(pcl::PointCloud::iterator it=pc->points.begin(); itpoints.end(); it++){ - double x_dif = (*it).x - center.x; - double y_dif = (*it).y - center.y; + // If lefttop != top_pt, swap indices + if (righttop_pt == top_pt) { + leftbottom_pt = max_dist; + rightbottom_pt = min_dist; + } + // Fill vector with sorted centers + v[0] = pc->points[lefttop_pt]; // lt + v[1] = pc->points[righttop_pt]; // rt + v[2] = pc->points[rightbottom_pt]; // rb + v[3] = pc->points[leftbottom_pt]; // lb +} +void colourCenters(const std::vector pc, + pcl::PointCloud::Ptr coloured) { + float intensity = 0; + for (int i = 0; i < 4; i++) { pcl::PointXYZI cc; - cc.x = (*it).x; - cc.y = (*it).y; - cc.z = (*it).z; - - if(x_dif < 0 && y_dif < 0){ - cc.intensity = 0; - }else if(x_dif > 0 && y_dif < 0){ - cc.intensity = 0.3; - }else if(x_dif > 0 && y_dif > 0){ - cc.intensity = 0.6; - }else{ - cc.intensity = 1; - } + cc.x = pc[i].x; + cc.y = pc[i].y; + cc.z = pc[i].z; + + cc.intensity = intensity; coloured->push_back(cc); + intensity += 0.3; } } -void getCenterClusters(pcl::PointCloud::Ptr cloud_in, pcl::PointCloud::Ptr centers_cloud, - double cluster_tolerance = 0.10, int min_cluster_size = 15, int max_cluster_size = 200, bool verbosity=true){ - - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (cloud_in); +void getCenterClusters(pcl::PointCloud::Ptr cloud_in, + pcl::PointCloud::Ptr centers_cloud, + double cluster_tolerance = 0.10, + int min_cluster_size = 15, int max_cluster_size = 200, + bool verbosity = true) { + pcl::search::KdTree::Ptr tree( + new pcl::search::KdTree); + tree->setInputCloud(cloud_in); std::vector cluster_indices; pcl::EuclideanClusterExtraction euclidean_cluster; - euclidean_cluster.setClusterTolerance (cluster_tolerance); - euclidean_cluster.setMinClusterSize (min_cluster_size); - euclidean_cluster.setMaxClusterSize (max_cluster_size); - euclidean_cluster.setSearchMethod (tree); - euclidean_cluster.setInputCloud (cloud_in); - euclidean_cluster.extract (cluster_indices); - - if(DEBUG && verbosity) cout << cluster_indices.size() << " clusters found from " << cloud_in->points.size() << " points in cloud" << endl; - - for (std::vector::iterator it=cluster_indices.begin(); itpoints.size() << " points in cloud" << endl; + + for (std::vector::iterator it = cluster_indices.begin(); + it < cluster_indices.end(); it++) { float accx = 0., accy = 0., accz = 0.; - for(vector::iterator it2=it->indices.begin(); it2indices.end(); it2++){ - accx+=cloud_in->at(*it2).x; - accy+=cloud_in->at(*it2).y; - accz+=cloud_in->at(*it2).z; + for (vector::iterator it2 = it->indices.begin(); + it2 < it->indices.end(); it2++) { + accx += cloud_in->at(*it2).x; + accy += cloud_in->at(*it2).y; + accz += cloud_in->at(*it2).z; } // Compute and add center to clouds pcl::PointXYZ center; - center.x = accx/it->indices.size(); - center.y = accy/it->indices.size(); - center.z = accz/it->indices.size(); + center.x = accx / it->indices.size(); + center.y = accy / it->indices.size(); + center.z = accz / it->indices.size(); centers_cloud->push_back(center); } } -Eigen::Affine3f getRotationMatrix(Eigen::Vector3f source, Eigen::Vector3f target){ +Eigen::Affine3f getRotationMatrix(Eigen::Vector3f source, + Eigen::Vector3f target) { Eigen::Vector3f rotation_vector = target.cross(source); rotation_vector.normalize(); - double theta = acos(source[2]/sqrt( pow(source[0],2)+ pow(source[1],2) + pow(source[2],2))); + double theta = acos(source[2] / sqrt(pow(source[0], 2) + pow(source[1], 2) + + pow(source[2], 2))); - if(DEBUG) ROS_INFO("Rot. vector: (%lf %lf %lf) / Angle: %lf", rotation_vector[0], rotation_vector[1], rotation_vector[2], theta); + if (DEBUG) + cout << "Rot. vector: " << rotation_vector << " / Angle: " << theta << endl; - Eigen::Matrix3f rotation = Eigen::AngleAxis(theta, rotation_vector) * Eigen::Scaling(1.0f); + Eigen::Matrix3f rotation = + Eigen::AngleAxis(theta, rotation_vector) * Eigen::Scaling(1.0f); Eigen::Affine3f rot(rotation); return rot; } +class Square { + private: + pcl::PointXYZ _center; + std::vector _candidates; + float _target_width, _target_height, _target_diagonal; + + public: + Square(std::vector candidates, float width, float height) { + _candidates = candidates; + _target_width = width; + _target_height = height; + _target_diagonal = sqrt(pow(width, 2) + pow(height, 2)); + + // Compute candidates centroid + for (int i = 0; i < candidates.size(); ++i) { + _center.x += candidates[i].x; + _center.y += candidates[i].y; + _center.z += candidates[i].z; + } + + _center.x /= candidates.size(); + _center.y /= candidates.size(); + _center.z /= candidates.size(); + } + + float distance(pcl::PointXYZ pt1, pcl::PointXYZ pt2) { + return sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2) + + pow(pt1.z - pt2.z, 2)); + } + + float perimeter() { // TODO: It is assumed that _candidates are ordered, it + // shouldn't + float perimeter = 0; + for (int i = 0; i < 4; ++i) { + perimeter += distance(_candidates[i], _candidates[(i + 1) % 4]); + } + return perimeter; + } + + pcl::PointXYZ at(int i) { + assert(0 <= i && i < 4); + return _candidates[i]; + } + + bool is_valid() { + pcl::PointCloud::Ptr candidates_cloud( + new pcl::PointCloud()); + // Check if candidates are at 5% of target's diagonal/2 to their centroid + for (int i = 0; i < _candidates.size(); ++i) { + candidates_cloud->push_back(_candidates[i]); + float d = distance(_center, _candidates[i]); + if (fabs(d - _target_diagonal / 2.) / (_target_diagonal / 2.) > + GEOMETRY_TOLERANCE) { + return false; + } + } + // Check perimeter? + std::vector sorted_centers; + sortPatternCenters(candidates_cloud, sorted_centers); + float perimeter = 0; + for (int i = 0; i < sorted_centers.size(); ++i) { + float current_distance = distance( + sorted_centers[i], sorted_centers[(i + 1) % sorted_centers.size()]); + if (i % 2) { + if (fabs(current_distance - _target_height) / _target_height > + GEOMETRY_TOLERANCE) { + return false; + } + } else { + if (fabs(current_distance - _target_width) / _target_width > + GEOMETRY_TOLERANCE) { + return false; + } + } + perimeter += current_distance; + } + float ideal_perimeter = (2 * _target_width + 2 * _target_height); + if (fabs((perimeter - ideal_perimeter) / ideal_perimeter > + GEOMETRY_TOLERANCE)) { + return false; + } + + // Check width + height? + return true; + } +}; + +void comb(int N, int K, std::vector> &groups) { + int upper_factorial = 1; + int lower_factorial = 1; + + for (int i = 0; i < K; i++) { + upper_factorial *= (N - i); + lower_factorial *= (K - i); + } + int n_permutations = upper_factorial / lower_factorial; + + if (DEBUG) + cout << N << " centers found. Iterating over " << n_permutations + << " possible sets of candidates" << endl; + + std::string bitmask(K, 1); // K leading 1's + bitmask.resize(N, 0); // N-K trailing 0's + + // print integers and permute bitmask + do { + std::vector group; + for (int i = 0; i < N; ++i) // [0..N-1] integers + { + if (bitmask[i]) { + group.push_back(i); + } + } + groups.push_back(group); + } while (std::prev_permutation(bitmask.begin(), bitmask.end())); + + assert(groups.size() == n_permutations); +} + +const std::string currentDateTime() { + time_t now = time(0); + struct tm tstruct; + char buf[80]; + tstruct = *localtime(&now); + strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct); + + return buf; +} + #endif diff --git a/launch/fake_tf.launch b/launch/fake_tf.launch deleted file mode 100644 index 6d868bb..0000000 --- a/launch/fake_tf.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/launch/full_calibration.launch b/launch/full_calibration.launch deleted file mode 100644 index 56eb7db..0000000 --- a/launch/full_calibration.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/launch/laser_pattern.launch b/launch/laser_pattern.launch deleted file mode 100644 index c17740e..0000000 --- a/launch/laser_pattern.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - filter_field_name: y - filter_limit_min: -100 - filter_limit_max: 100 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - - - filter_field_name: x - filter_limit_min: 0 - filter_limit_max: 100 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - x: 0 - y: 0 - z: 1 - angle_threshold: 0.55 - circle_radius: 0.12 - passthrough_radius_min: 1.0 - passthrough_radius_max: 6.0 - - - - diff --git a/launch/levinson.launch b/launch/levinson.launch deleted file mode 100644 index d182bbf..0000000 --- a/launch/levinson.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/launch/levinson_edges.launch b/launch/levinson_edges.launch deleted file mode 100644 index 8784be8..0000000 --- a/launch/levinson_edges.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/lidar_pattern.launch b/launch/lidar_pattern.launch new file mode 100644 index 0000000..0906230 --- /dev/null +++ b/launch/lidar_pattern.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + filter_field_name: z + filter_limit_min: -2 + filter_limit_max: 1 + filter_limit_negative: False + max_queue_size: 1 + + + + + + + + filter_field_name: y + filter_limit_min: -100 + filter_limit_max: 100 + filter_limit_negative: False + max_queue_size: 1 + + + + + + + + filter_field_name: x + filter_limit_min: 0 + filter_limit_max: 100 + filter_limit_negative: False + max_queue_size: 1 + + + + + + + passthrough_radius_min: 1.0 + passthrough_radius_max: 6.0 + + + + diff --git a/launch/mono_pattern.launch b/launch/mono_pattern.launch new file mode 100644 index 0000000..1db2958 --- /dev/null +++ b/launch/mono_pattern.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/launch/pcl_coloring.launch b/launch/pcl_coloring.launch deleted file mode 100644 index b277e92..0000000 --- a/launch/pcl_coloring.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/pcl_projection.launch b/launch/pcl_projection.launch deleted file mode 100644 index 78757fe..0000000 --- a/launch/pcl_projection.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/real_laser_pattern.launch b/launch/real_laser_pattern.launch deleted file mode 100644 index af34157..0000000 --- a/launch/real_laser_pattern.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - filter_field_name: y - filter_limit_min: -1.5 - filter_limit_max: 1.5 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - - - filter_field_name: x - filter_limit_min: 1 - filter_limit_max: 15 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - x: 0 - y: 0 - z: 1 - angle_threshold: 0.55 - circle_radius: 0.12 - passthrough_radius_min: 1.5 - passthrough_radius_max: 5.5 - - - - diff --git a/launch/real_stereo_pattern.launch b/launch/real_stereo_pattern.launch deleted file mode 100644 index 81a4627..0000000 --- a/launch/real_stereo_pattern.launch +++ /dev/null @@ -1,138 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - filter_field_name: z - filter_limit_min: 2 - filter_limit_max: 4 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - - - - filter_field_name: z - filter_limit_min: 2 - filter_limit_max: 4 - filter_limit_negative: False - max_queue_size: 1 - keep_organized: False - - - - - - - - segmentation_type: 1 - axis: [0.0, 1.0, 0.0] - threshold: 0.01 - eps_angle: 0.6 - - - - - - - - - negative: false - - - - - - - - - - - - - - - - - - - - - - negative: false - - - - - diff --git a/launch/registration.launch b/launch/registration.launch new file mode 100644 index 0000000..ab06f4a --- /dev/null +++ b/launch/registration.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/stereo_pattern.launch b/launch/stereo_pattern.launch index ce1a3c2..a71ff2d 100644 --- a/launch/stereo_pattern.launch +++ b/launch/stereo_pattern.launch @@ -1,57 +1,67 @@ + + + + - + - + - - - - - - - - - - - - + args="load stereo_image_proc/disparity stereo_manager_$(arg sensor_id)"> + + disparity_range: 128 + correlation_window_size: 5 + stereo_algorithm: 1 + prefilter_cap: 63 + uniqueness_ratio: 10.0 + speckle_size: 100 + speckle_range: 2 + P1: 216 + P2: 864 + disp12MaxDiff: 1 + + + + args="load stereo_image_proc/point_cloud2 stereo_manager_$(arg sensor_id) _queue_size:=100"> - - - - - - - - - - - - + + + use_camera_info: False + debug_view: False + edge_type: 0 + canny_threshold1: 100 + canny_threshold2: 200 + apertureSize: 3 + apply_blur_pre: True + postBlurSize: 13 + postBlurSigma: 3.2 + apply_blur_post: False + L2gradient: False + - + - - + + + edges_threshold: 128 + - + - + - - - + + + filter_field_name: z filter_limit_min: 0.8 filter_limit_max: 3 - filter_limit_negative: False + filter_limit_negative: false max_queue_size: 1 - keep_organized: False + keep_organized: false - - - + + + filter_field_name: z filter_limit_min: 0.8 filter_limit_max: 3 - filter_limit_negative: False + filter_limit_negative: false max_queue_size: 1 - keep_organized: False + keep_organized: false - + - + segmentation_type: 1 axis: [0.0, 1.0, 0.0] - threshold: 0.01 + threshold: 0.1 eps_angle: 0.55 - - + + negative: false @@ -114,18 +124,13 @@ - - + + - - - - - - + - @@ -133,5 +138,5 @@ - + diff --git a/launch/velo2cam_calibration.launch b/launch/velo2cam_calibration.launch deleted file mode 100644 index 3e22b91..0000000 --- a/launch/velo2cam_calibration.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/screenshots/calibration_target_real_scheme_journal.png b/screenshots/calibration_target_real_scheme_journal.png new file mode 100644 index 0000000..cda9009 Binary files /dev/null and b/screenshots/calibration_target_real_scheme_journal.png differ diff --git a/screenshots/calibration_target_scheme.png b/screenshots/calibration_target_scheme.png deleted file mode 100644 index 559eb7e..0000000 Binary files a/screenshots/calibration_target_scheme.png and /dev/null differ diff --git a/screenshots/coordinates_frames.png b/screenshots/coordinates_frames.png deleted file mode 100644 index 9a77fbc..0000000 Binary files a/screenshots/coordinates_frames.png and /dev/null differ diff --git a/screenshots/lidar_filters_1.png b/screenshots/lidar_filters_1.png new file mode 100644 index 0000000..2971953 Binary files /dev/null and b/screenshots/lidar_filters_1.png differ diff --git a/screenshots/lidar_filters_2.png b/screenshots/lidar_filters_2.png new file mode 100644 index 0000000..f465219 Binary files /dev/null and b/screenshots/lidar_filters_2.png differ diff --git a/screenshots/real_results.png b/screenshots/real_results.png new file mode 100644 index 0000000..53d9229 Binary files /dev/null and b/screenshots/real_results.png differ diff --git a/screenshots/stereo_filters_1.png b/screenshots/stereo_filters_1.png new file mode 100644 index 0000000..63cdc45 Binary files /dev/null and b/screenshots/stereo_filters_1.png differ diff --git a/screenshots/stereo_filters_2.png b/screenshots/stereo_filters_2.png new file mode 100644 index 0000000..2a1003d Binary files /dev/null and b/screenshots/stereo_filters_2.png differ diff --git a/screenshots/velo2cam_calibration_setup.png b/screenshots/velo2cam_calibration_setup.png deleted file mode 100644 index 012e936..0000000 Binary files a/screenshots/velo2cam_calibration_setup.png and /dev/null differ diff --git a/src/disp_masker.cpp b/src/disp_masker.cpp index 7dcf74a..91e0720 100644 --- a/src/disp_masker.cpp +++ b/src/disp_masker.cpp @@ -1,6 +1,7 @@ /* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel This file is part of velo2cam_calibration. @@ -22,20 +23,17 @@ disp_masker: Mask the disparity map according to the edges image */ -#include +#include #include -#include -#include #include -#include -#include -#include +#include #include #include -class Masker -{ -public: +#include + +class Masker { + public: ros::NodeHandle nh_; message_filters::Subscriber image_sub_; @@ -45,81 +43,81 @@ class Masker bool isfreeobs_; int edges_threshold_; - typedef message_filters::sync_policies::ExactTime ExSync; + typedef message_filters::sync_policies::ExactTime + ExSync; message_filters::Synchronizer sync; - Masker(): - nh_("~"), - image_sub_(nh_, "image", 1), - mask_sub_(nh_, "mask", 1), - sync(ExSync(100), image_sub_, mask_sub_) - { - + Masker() + : nh_("~"), + image_sub_(nh_, "image", 1), + mask_sub_(nh_, "mask", 1), + sync(ExSync(100), image_sub_, mask_sub_) { masked_pub_ = nh_.advertise("output", 1); nh_.param("isFreeobs", isfreeobs_, false); nh_.param("edges_threshold", edges_threshold_, 16); sync.registerCallback(boost::bind(&Masker::callback, this, _1, _2)); - } - void callback(const stereo_msgs::DisparityImageConstPtr& disp, const sensor_msgs::ImageConstPtr& ma) - { + void callback(const stereo_msgs::DisparityImageConstPtr &disp, + const sensor_msgs::ImageConstPtr &ma) { cv::Mat mask, binary_mask, output; cv_bridge::CvImageConstPtr cv_im; - try - { - cv_im = cv_bridge::toCvShare(disp->image, disp, sensor_msgs::image_encodings::TYPE_32FC1); + try { + cv_im = cv_bridge::toCvShare(disp->image, disp, + sensor_msgs::image_encodings::TYPE_32FC1); mask = cv_bridge::toCvShare(ma, "mono8")->image; - } - catch (cv_bridge::Exception& e) - { + } catch (cv_bridge::Exception &e) { ROS_ERROR("CvBridge failed"); } static cv::Mat disparity32(cv_im->image.rows, cv_im->image.cols, CV_32FC1); disparity32 = cv_im->image; - if (isfreeobs_){ + if (isfreeobs_) { const static int OBSTACLE_LABEL = 32; - cv::Mat obs_pattern(mask.rows, mask.cols, CV_8UC1, cv::Scalar(OBSTACLE_LABEL)); + cv::Mat obs_pattern(mask.rows, mask.cols, CV_8UC1, + cv::Scalar(OBSTACLE_LABEL)); cv::bitwise_and(mask, obs_pattern, binary_mask); - binary_mask = binary_mask * (255.0/OBSTACLE_LABEL); - }else{ + binary_mask = binary_mask * (255.0 / OBSTACLE_LABEL); + } else { cv::threshold(mask, binary_mask, edges_threshold_, 255, 0); } // Copy input disparity to another DisparityImage variable - stereo_msgs::DisparityImagePtr copy_disp = boost::make_shared (); - copy_disp->valid_window.x_offset = disp->valid_window.x_offset; - copy_disp->valid_window.y_offset = disp->valid_window.y_offset; - copy_disp->valid_window.width = disp->valid_window.width; - copy_disp->valid_window.height = disp->valid_window.height; - copy_disp->header = disp->header; - copy_disp->image.header = disp->header; - copy_disp->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - copy_disp->image.height = disp->image.height; - copy_disp->image.width = disp->image.width; - copy_disp->image.step = disp->image.step; - copy_disp->T = disp->T; - copy_disp->f = disp->f; - - copy_disp->min_disparity = disp->min_disparity; - copy_disp->max_disparity = disp->max_disparity; - copy_disp->delta_d = disp->delta_d; + stereo_msgs::DisparityImagePtr copy_disp = + boost::make_shared(); + copy_disp->valid_window.x_offset = disp->valid_window.x_offset; + copy_disp->valid_window.y_offset = disp->valid_window.y_offset; + copy_disp->valid_window.width = disp->valid_window.width; + copy_disp->valid_window.height = disp->valid_window.height; + copy_disp->header = disp->header; + copy_disp->image.header = disp->header; + copy_disp->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + copy_disp->image.height = disp->image.height; + copy_disp->image.width = disp->image.width; + copy_disp->image.step = disp->image.step; + copy_disp->T = disp->T; + copy_disp->f = disp->f; + + copy_disp->min_disparity = disp->min_disparity; + copy_disp->max_disparity = disp->max_disparity; + copy_disp->delta_d = disp->delta_d; // Create cv::Mat from the copies DisparityImage input - sensor_msgs::Image& d_image = copy_disp->image; - d_image.height = disparity32.rows; - d_image.width = disparity32.cols; - d_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - d_image.step = d_image.width * sizeof(float); + sensor_msgs::Image &d_image = copy_disp->image; + d_image.height = disparity32.rows; + d_image.width = disparity32.cols; + d_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + d_image.step = d_image.width * sizeof(float); d_image.data.resize(d_image.step * d_image.height); - cv::Mat_ dmat(d_image.height, d_image.width, (float*)&d_image.data[0], d_image.step); + cv::Mat_ dmat(d_image.height, d_image.width, + (float *)&d_image.data[0], d_image.step); // Check data ROS_ASSERT(dmat.data == &d_image.data[0]); @@ -129,11 +127,9 @@ class Masker // Publish obstacle disparity masked_pub_.publish(copy_disp); } - }; -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "image_masker"); Masker im; diff --git a/src/laser_pattern.cpp b/src/laser_pattern.cpp deleted file mode 100644 index 89d5f3f..0000000 --- a/src/laser_pattern.cpp +++ /dev/null @@ -1,474 +0,0 @@ -/* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel - - This file is part of velo2cam_calibration. - - velo2cam_calibration 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 2 of the License, or - (at your option) any later version. - - velo2cam_calibration 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 velo2cam_calibration. If not, see . -*/ - -/* - laser_pattern: Find the circle centers in the laser cloud -*/ - -#define PCL_NO_PRECOMPILE - -#include -#include "ros/package.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "velo2cam_utils.h" -#include - -using namespace std; -using namespace sensor_msgs; - -ros::Publisher cumulative_pub, centers_pub, pattern_pub, range_pub, - coeff_pub, aux_pub, auxpoint_pub, debug_pub; -int nFrames; // Used for resetting center computation -pcl::PointCloud::Ptr cumulative_cloud; - -// Dynamic parameters -double threshold_; -double passthrough_radius_min_, passthrough_radius_max_, circle_radius_, - centroid_distance_min_, centroid_distance_max_; -Eigen::Vector3f axis_; -double angle_threshold_; -double cluster_size_; -int clouds_proc_ = 0, clouds_used_ = 0; -int min_centers_found_; - -void callback(const PointCloud2::ConstPtr& laser_cloud){ - - if(DEBUG) ROS_INFO("[Laser] Processing cloud..."); - - pcl::PointCloud::Ptr velocloud (new pcl::PointCloud), - velo_filtered(new pcl::PointCloud), - pattern_cloud(new pcl::PointCloud); - - clouds_proc_++; - - fromROSMsg(*laser_cloud, *velocloud); - - Velodyne::addRange(*velocloud); // For latter computation of edge detection - - pcl::PointCloud::Ptr radius(new pcl::PointCloud); - pcl::PassThrough pass2; - pass2.setInputCloud (velocloud); - pass2.setFilterFieldName ("range"); - pass2.setFilterLimits (passthrough_radius_min_, passthrough_radius_max_); - pass2.filter (*velo_filtered); - - sensor_msgs::PointCloud2 range_ros; - pcl::toROSMsg(*velo_filtered, range_ros); - range_ros.header = laser_cloud->header; - range_pub.publish(range_ros); - - // Plane segmentation - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); - - pcl::SACSegmentation plane_segmentation; - plane_segmentation.setModelType (pcl::SACMODEL_PARALLEL_PLANE); - plane_segmentation.setDistanceThreshold (0.01); - plane_segmentation.setMethodType (pcl::SAC_RANSAC); - plane_segmentation.setAxis(Eigen::Vector3f(axis_[0], axis_[1], axis_[2])); - plane_segmentation.setEpsAngle (angle_threshold_); - plane_segmentation.setOptimizeCoefficients (true); - plane_segmentation.setMaxIterations(1000); - plane_segmentation.setInputCloud (velo_filtered); - plane_segmentation.segment (*inliers, *coefficients); - - if (inliers->indices.size () == 0) - { - ROS_WARN("[Laser] Could not estimate a planar model for the given dataset."); - return; - } - - // Copy coefficients to proper object for further filtering - Eigen::VectorXf coefficients_v(4); - coefficients_v(0) = coefficients->values[0]; - coefficients_v(1) = coefficients->values[1]; - coefficients_v(2) = coefficients->values[2]; - coefficients_v(3) = coefficients->values[3]; - - // Get edges points by range - vector > rings = Velodyne::getRings(*velocloud); - for (vector >::iterator ring = rings.begin(); ring < rings.end(); ++ring){ - if (ring->empty()) continue; - - (*ring->begin())->intensity = 0; - (*(ring->end() - 1))->intensity = 0; - for (vector::iterator pt = ring->begin() + 1; pt < ring->end() - 1; pt++){ - Velodyne::Point *prev = *(pt - 1); - Velodyne::Point *succ = *(pt + 1); - (*pt)->intensity = max( max( prev->range-(*pt)->range, succ->range-(*pt)->range), 0.f); - } - } - - pcl::PointCloud::Ptr edges_cloud(new pcl::PointCloud); - float THRESHOLD = 0.5 ; - for (pcl::PointCloud::iterator pt = velocloud->points.begin(); pt < velocloud->points.end(); ++pt){ - if(pt->intensity>THRESHOLD){ - edges_cloud->push_back(*pt); - } - } - - if (edges_cloud->points.size () == 0) - { - ROS_WARN("[Laser] Could not detect pattern edges."); - return; - } - - // Get points belonging to plane in pattern pointcloud - pcl::SampleConsensusModelPlane::Ptr dit (new pcl::SampleConsensusModelPlane (edges_cloud)); - std::vector inliers2; - dit -> selectWithinDistance (coefficients_v, .05, inliers2); // 0.1 - pcl::copyPointCloud(*edges_cloud, inliers2, *pattern_cloud); - - // Remove kps not belonging to circles by coords - pcl::PointCloud::Ptr circles_cloud(new pcl::PointCloud); - vector > rings2 = Velodyne::getRings(*pattern_cloud); - int ringsWithCircle = 0; - for (vector >::iterator ring = rings2.begin(); ring < rings2.end(); ++ring){ - if(ring->size() < 4){ - ring->clear(); - }else{ // Remove first and last points in ring - ringsWithCircle++; - ring->erase(ring->begin()); - ring->pop_back(); - - for (vector::iterator pt = ring->begin(); pt < ring->end(); ++pt){ - // Velodyne specific info no longer needed for calibration - // so standard point is used from now on - pcl::PointXYZ point; - point.x = (*pt)->x; - point.y = (*pt)->y; - point.z = (*pt)->z; - circles_cloud->push_back(point); - } - } - } - - if(circles_cloud->points.size() > ringsWithCircle*4){ - ROS_WARN("[Laser] Too many outliers, not computing circles."); - return; - } - - sensor_msgs::PointCloud2 velocloud_ros2; - pcl::toROSMsg(*circles_cloud, velocloud_ros2); - velocloud_ros2.header = laser_cloud->header; - pattern_pub.publish(velocloud_ros2); - - // Rotate cloud to face pattern plane - pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); - Eigen::Vector3f xy_plane_normal_vector, floor_plane_normal_vector; - xy_plane_normal_vector[0] = 0.0; - xy_plane_normal_vector[1] = 0.0; - xy_plane_normal_vector[2] = -1.0; - - floor_plane_normal_vector[0] = coefficients->values[0]; - floor_plane_normal_vector[1] = coefficients->values[1]; - floor_plane_normal_vector[2] = coefficients->values[2]; - - Eigen::Affine3f rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector); - pcl::transformPointCloud(*circles_cloud, *xy_cloud, rotation); - - pcl::PointCloud::Ptr aux_cloud(new pcl::PointCloud); - pcl::PointXYZ aux_point; - aux_point.x = 0; - aux_point.y = 0; - aux_point.z = (-coefficients_v(3)/coefficients_v(2)); - aux_cloud->push_back(aux_point); - - pcl::PointCloud::Ptr auxrotated_cloud(new pcl::PointCloud); - pcl::transformPointCloud(*aux_cloud, *auxrotated_cloud, rotation); - - sensor_msgs::PointCloud2 ros_auxpoint; - pcl::toROSMsg(*auxrotated_cloud, ros_auxpoint); - ros_auxpoint.header = laser_cloud->header; - auxpoint_pub.publish(ros_auxpoint); - - double zcoord_xyplane = auxrotated_cloud->at(0).z; - - pcl::PointXYZ edges_centroid; - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud (xy_cloud); - - std::vector cluster_indices; - pcl::EuclideanClusterExtraction euclidean_cluster; - euclidean_cluster.setClusterTolerance (0.55); - euclidean_cluster.setMinClusterSize (12); - euclidean_cluster.setMaxClusterSize (RINGS_COUNT*4); - euclidean_cluster.setSearchMethod (tree); - euclidean_cluster.setInputCloud (xy_cloud); - euclidean_cluster.extract (cluster_indices); - - //if(DEBUG) cout << cluster_indices.size() << " clusters found from " << xy_cloud->points.size() << " points in cloud" << endl; - - for (std::vector::iterator it=cluster_indices.begin(); it::iterator it2=it->indices.begin(); it2indices.end(); ++it2){ - accx+=xy_cloud->at(*it2).x; - accy+=xy_cloud->at(*it2).y; - accz+=xy_cloud->at(*it2).z; - } - // Compute and add center to clouds - edges_centroid.x = accx/it->indices.size(); - edges_centroid.y = accy/it->indices.size(); - edges_centroid.z = accz/it->indices.size(); - //if(DEBUG) ROS_INFO("Centroid %f %f %f", edges_centroid.x, edges_centroid.y, edges_centroid.z); - } - - // Extract circles - pcl::ModelCoefficients::Ptr coefficients3 (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers3 (new pcl::PointIndices); - - // Ransac settings for circle detecstion - pcl::SACSegmentation circle_segmentation; - circle_segmentation.setModelType (pcl::SACMODEL_CIRCLE2D); - circle_segmentation.setDistanceThreshold (0.04); - circle_segmentation.setMethodType (pcl::SAC_RANSAC); - circle_segmentation.setOptimizeCoefficients (true); - circle_segmentation.setMaxIterations(1000); - circle_segmentation.setRadiusLimits(circle_radius_- 0.02, circle_radius_+ 0.02); - - pcl::PointCloud::Ptr copy_cloud(new pcl::PointCloud); // Used for removing inliers - pcl::copyPointCloud(*xy_cloud, *copy_cloud); - pcl::PointCloud::Ptr circle_cloud(new pcl::PointCloud); // To store circle points - pcl::PointCloud::Ptr centroid_cloud(new pcl::PointCloud); // To store circle points - pcl::PointCloud::Ptr cloud_f(new pcl::PointCloud); // Temp pc used for swaping - - // Force pattern points to belong to computed plane - for (pcl::PointCloud::iterator pt = copy_cloud->points.begin(); pt < copy_cloud->points.end(); ++pt){ - pt->z = zcoord_xyplane; - } - - pcl::ExtractIndices extract; - - std::vector< std::vector > found_centers; - std::vector centroid_cloud_inliers; - bool valid = true; - - while ((copy_cloud->points.size()+centroid_cloud_inliers.size()) > 3 && found_centers.size()<4 && copy_cloud->points.size()){ - circle_segmentation.setInputCloud (copy_cloud); - circle_segmentation.segment (*inliers3, *coefficients3); - if (inliers3->indices.size () == 0) - { - break; - } - - // Extract the inliers - extract.setInputCloud (copy_cloud); - extract.setIndices (inliers3); - extract.setNegative (false); - extract.filter (*circle_cloud); - - sensor_msgs::PointCloud2 range_ros2; - pcl::toROSMsg(*circle_cloud, range_ros2); - range_ros2.header = laser_cloud->header; - debug_pub.publish(range_ros2); - - // Add center point to cloud - pcl::PointXYZ center; - center.x = *coefficients3->values.begin(); - center.y = *(coefficients3->values.begin()+1); - center.z = zcoord_xyplane; - // Make sure there is no circle at the center of the pattern or far away from it - double centroid_distance = sqrt(pow(fabs(edges_centroid.x-center.x),2) + pow(fabs(edges_centroid.y-center.y),2)); - // if(DEBUG) ROS_INFO("Distance to centroid %f", centroid_distance); - if (centroid_distance < centroid_distance_min_){ - valid = false; - for (pcl::PointCloud::iterator pt = circle_cloud->points.begin(); pt < circle_cloud->points.end(); ++pt){ - centroid_cloud_inliers.push_back(*pt); - } - }else if(centroid_distance > centroid_distance_max_){ - valid = false; - }else{ - // if(DEBUG) ROS_INFO("Valid centroid"); - for(std::vector >::iterator it = found_centers.begin(); it != found_centers.end(); ++it) { - // if(DEBUG) ROS_INFO("%f", sqrt(pow(fabs((*it)[0]-center.x),2) + pow(fabs((*it)[1]-center.y),2))); - if (sqrt(pow(fabs((*it)[0]-center.x),2) + pow(fabs((*it)[1]-center.y),2))<0.25){ - valid = false; - break; - } - } - - // If center is valid, check if any point from wrong_circle belongs to it, and pop it if true - for (std::vector::iterator pt = centroid_cloud_inliers.begin(); pt < centroid_cloud_inliers.end(); ++pt){ - pcl::PointXYZ schrodinger_pt((*pt).x, (*pt).y, (*pt).z); - double distance_to_cluster = sqrt(pow(schrodinger_pt.x-center.x,2) + pow(schrodinger_pt.y-center.y,2) + pow(schrodinger_pt.z-center.z,2)); - // if(DEBUG) ROS_INFO("Distance to cluster: %lf", distance_to_cluster); - if(distance_to_cluster found_center; - found_center.push_back(center.x); - found_center.push_back(center.y); - found_center.push_back(center.z); - found_centers.push_back(found_center); - // if(DEBUG) ROS_INFO("Remaining points in cloud %lu", copy_cloud->points.size()); - } - - // Remove inliers from pattern cloud to find next circle - extract.setNegative (true); - extract.filter(*cloud_f); - copy_cloud.swap(cloud_f); - valid = true; - - if(DEBUG) ROS_INFO("Remaining points in cloud %lu", copy_cloud->points.size()); - } - - if(found_centers.size() >= min_centers_found_ && found_centers.size() < 5){ - for (std::vector >::iterator it = found_centers.begin(); it < found_centers.end(); ++it){ - pcl::PointXYZ center; - center.x = (*it)[0]; - center.y = (*it)[1]; - center.z = (*it)[2];; - pcl::PointXYZ center_rotated_back = pcl::transformPoint(center, rotation.inverse()); - center_rotated_back.x = (- coefficients->values[1] * center_rotated_back.y - coefficients->values[2] * center_rotated_back.z - coefficients->values[3])/coefficients->values[0]; - cumulative_cloud->push_back(center_rotated_back); - } - - sensor_msgs::PointCloud2 ros_pointcloud; - pcl::toROSMsg(*cumulative_cloud, ros_pointcloud); - ros_pointcloud.header = laser_cloud->header; - cumulative_pub.publish(ros_pointcloud); - }else{ - ROS_WARN("[Laser] Not enough centers: %ld", found_centers.size()); - return; - } - - circle_cloud.reset(); - copy_cloud.reset(); // Free memory - cloud_f.reset(); // Free memory - - nFrames++; - clouds_used_ = nFrames; - - pcl_msgs::ModelCoefficients m_coeff; - pcl_conversions::moveFromPCL(*coefficients, m_coeff); - m_coeff.header = laser_cloud->header; - coeff_pub.publish(m_coeff); - - ROS_INFO("[Laser] %d/%d frames: %ld pts in cloud", clouds_used_, clouds_proc_, cumulative_cloud->points.size()); - - // Create cloud for publishing centers - pcl::PointCloud::Ptr centers_cloud(new pcl::PointCloud); - - // Compute circles centers - getCenterClusters(cumulative_cloud, centers_cloud, cluster_size_, nFrames/2, nFrames); - if (centers_cloud->points.size()>4){ - getCenterClusters(cumulative_cloud, centers_cloud, cluster_size_, 3.0*nFrames/4.0, nFrames); - } - - if (centers_cloud->points.size()==4){ - - sensor_msgs::PointCloud2 ros2_pointcloud; - pcl::toROSMsg(*centers_cloud, ros2_pointcloud); - ros2_pointcloud.header = laser_cloud->header; - - velo2cam_calibration::ClusterCentroids to_send; - to_send.header = laser_cloud->header; - to_send.cluster_iterations = clouds_used_; - to_send.total_iterations = clouds_proc_; - to_send.cloud = ros2_pointcloud; - - centers_pub.publish(to_send); - //if(DEBUG) ROS_INFO("Pattern centers published"); - } -} - -void param_callback(velo2cam_calibration::LaserConfig &config, uint32_t level){ - passthrough_radius_min_ = config.passthrough_radius_min; - ROS_INFO("New passthrough_radius_min_ threshold: %f", passthrough_radius_min_); - passthrough_radius_max_ = config.passthrough_radius_max; - ROS_INFO("New passthrough_radius_max_ threshold: %f", passthrough_radius_max_); - circle_radius_ = config.circle_radius; - ROS_INFO("New pattern circle radius: %f", circle_radius_); - axis_[0] = config.x; - axis_[1] = config.y; - axis_[2] = config.z; - ROS_INFO("New normal axis for plane segmentation: %f, %f, %f", axis_[0], axis_[1], axis_[2]); - angle_threshold_ = config.angle_threshold; - ROS_INFO("New angle threshold: %f", angle_threshold_); - centroid_distance_min_ = config.centroid_distance_min; - ROS_INFO("New minimum distance between centroids: %f", centroid_distance_min_); - centroid_distance_max_ = config.centroid_distance_max; - ROS_INFO("New maximum distance between centroids: %f", centroid_distance_max_); -} - -int main(int argc, char **argv){ - ros::init(argc, argv, "laser_pattern"); - ros::NodeHandle nh_("~"); // LOCAL - ros::Subscriber sub = nh_.subscribe ("cloud1", 1, callback); - - range_pub = nh_.advertise ("range_filtered_velo", 1); - pattern_pub = nh_.advertise ("pattern_circles", 1); - auxpoint_pub = nh_.advertise ("rotated_pattern", 1); - cumulative_pub = nh_.advertise ("cumulative_cloud", 1); - centers_pub = nh_.advertise ("centers_cloud", 1); - - debug_pub = nh_.advertise ("debug", 1); - - coeff_pub = nh_.advertise ("plane_model", 1); - - nh_.param("cluster_size", cluster_size_, 0.02); - nh_.param("min_centers_found", min_centers_found_, 4); - - nFrames = 0; - cumulative_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(param_callback, _1, _2); - server.setCallback(f); - - ros::spin(); - return 0; -} diff --git a/src/levinson.cpp b/src/levinson.cpp deleted file mode 100644 index 195d908..0000000 --- a/src/levinson.cpp +++ /dev/null @@ -1,574 +0,0 @@ -/* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel - - This file is part of velo2cam_calibration. - - velo2cam_calibration 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 2 of the License, or - (at your option) any later version. - - velo2cam_calibration 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 velo2cam_calibration. If not, see . -*/ - -/* - Includes code from https://github.com/Flos/ros-image_cloud licensed - under the MIT license, set out below. - - The MIT License (MIT) - - Copyright (c) 2015 - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -/* - Calculates miscalibration value according to: - J. Levinson and S. Thrun, “Automatic Online Calibration of Cameras and Lasers,” - in Robotics: Science and Systems, 2013. -*/ - -#define PCL_NO_PRECOMPILE - -#include "velo2cam_utils.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -bool laserReceived, cameraReceived; -pcl::PointCloud::Ptr edges_cloud; -ros::Publisher cloud_pub; -cv::Mat img_out; -std::string target_frame, source_frame; -bool listen_to_tf_, save_to_file_; -int step_; -std::ofstream savefile; - -typedef struct { - double r,g,b; -} COLOUR; - -COLOUR GetColour(double v,double vmin,double vmax) -{ - COLOUR c = {1.0,1.0,1.0}; // white - double dv; - - if (v < vmin) - v = vmin; - if (v > vmax) - v = vmax; - dv = vmax - vmin; - - if (v < (vmin + 0.25 * dv)) { - c.r = 0; - c.g = 4 * (v - vmin) / dv; - } else if (v < (vmin + 0.5 * dv)) { - c.r = 0; - c.b = 1 + 4 * (vmin + 0.25 * dv - v) / dv; - } else if (v < (vmin + 0.75 * dv)) { - c.r = 4 * (v - vmin - 0.5 * dv) / dv; - c.b = 0; - } else { - c.g = 1 + 4 * (vmin + 0.75 * dv - v) / dv; - c.b = 0; - } - - return(c); -} - -void edges_pointcloud(pcl::PointCloud::Ptr pc){ - std::vector > rings = Velodyne::getRings(*pc); - for (std::vector >::iterator ring = rings.begin(); ring < rings.end(); ++ring){ - if (ring->empty()) continue; - - (*ring->begin())->intensity = 0; - (*(ring->end() - 1))->intensity = 0; - for (vector::iterator pt = ring->begin() + 1; pt < ring->end() - 1; ++pt){ - Velodyne::Point *prev = *(pt - 1); - Velodyne::Point *succ = *(pt + 1); - (*pt)->intensity = pow(std::max( std::max( prev->range-(*pt)->range, succ->range-(*pt)->range), 0.f), 0.5); - } - } -} - -template -inline bool between(T min, T test, T max){ - if( min < test && test < max ) return true; - return false; -} - -template -inline -void get_diff(int col, int row, int col_c, int row_c, int &result, cv::Mat& in) { - if (between(-1, col, in.cols) && between(-1, row, in.rows)) { - // hit upper left - result = std::max(abs(in.at(row, col) - in.at(row_c, col_c)), result); - } -} - -template -inline -int max_diff_neighbors(int row_c, int col_c, cv::Mat &in){ - //Check - int result = 0; - - get_diff(col_c -1, row_c -1, col_c, row_c, result, in); - get_diff(col_c , row_c -1, col_c, row_c, result, in); - get_diff(col_c +1, row_c -1, col_c, row_c, result, in); - get_diff(col_c -1, row_c , col_c, row_c, result, in); - get_diff(col_c +1, row_c , col_c, row_c, result, in); - get_diff(col_c -1, row_c +1, col_c, row_c, result, in); - get_diff(col_c , row_c +1, col_c, row_c, result, in); - get_diff(col_c +1, row_c +1, col_c, row_c, result, in); - - return result; -} - -template -inline void -edge_max(cv::Mat &in, cv::Mat &out) -{ - assert(in.rows == out.rows); - assert(in.cols == out.cols); - assert(in.depth() == out.depth()); - assert(in.channels() == out.channels()); - - for(int r = 0; r < in.rows; r++) - { - for(int c = 0; c < in.cols; c++) - { - out.at(r,c) = max_diff_neighbors(r, c, in); - } - } -} - -template -inline float calc(float &val, const float &psi, int row, int col, const cv::Mat& in, cv::Mat& out) { - - val = val * psi; /* Fade out the value */ - - if (in.at(row, col) > val) /* In Value in the image bigger than the current value */ - { - val = in.at(row, col); /* yes, get the bigger value */ - } - - if (out.at(row, col) < val) /* is the calculated value bigger then the value in the filtered image? */ - { - out.at(row, col) = val; /* yes, store the calculated value in the filtered image */ - } - else{ - val = out.at(row, col); /* no, the value of the filtered image is bigger use it */ - } - - return val; -} - -template -inline void -neighbors_x_pos(cv::Mat &in, cv::Mat &out, float psi, float alpha){ - for(int row = 0; row < in.rows; ++row) - { - float val = 0; - val = 0; - for(int col = 0; col < in.cols; ++col) - { - val = calc(val, psi, row, col, in, out); - } - } -} - -template -inline void -neighbors_x_neg(cv::Mat &in, cv::Mat &out, float psi, float alpha){ - float val = 0; - - for(int row = 0; row < in.rows; ++row) - { - val = 0; - for(int col = in.cols -1; col >= 0; --col) - { - val = calc(val, psi, row, col, in, out); - } - } -} - -template -inline void -neighbors_y_pos(cv::Mat &in, cv::Mat &out, float psi, float alpha){ - float val = 0; - - for(int col = 0; col < in.cols; ++col) - { - val = 0; - for(int row = 0; row < in.rows; ++row) - { - val = calc(val, psi, row, col, in, out); - } - } -} - -template -inline void -neighbors_y_neg(cv::Mat &in, cv::Mat &out, float psi, float alpha){ - float val = 0; - - for(int col = 0; col < in.cols; ++col) - { - val = 0; - for(int row = in.rows -1; row >= 0; --row) - { - val = calc(val, psi, row, col, in, out); - } - } -} - -template -void -inverse_distance_transformation(cv::Mat &in, cv::Mat &out, float alpha = 0.333333333, float psi = 0.98) -{ - assert(in.channels() == 1); - assert(in.depth() == CV_8U); - - assert(in.size == out.size); - assert(in.rows == out.rows); - assert(in.cols == out.cols); - - neighbors_x_pos(in, out, psi, alpha); - neighbors_x_neg(in, out, psi, alpha); - neighbors_y_pos(in, out, psi, alpha); - neighbors_y_neg(in, out, psi, alpha); - - for(int row = 0; row < in.rows; row++) - { - for(int col = 0; col < in.cols; col++) - { - int val = alpha * in.at(row,col) + (1 - alpha)*(float)out.at(row,col); - out.at(row, col) = val; - } - } -} - -void -transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) -{ - double mv[12]; - bt.getBasis ().getOpenGLSubMatrix (mv); - - tf::Vector3 origin = bt.getOrigin (); - - out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; - out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; - out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; - - out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; - out_mat (0, 3) = origin.x (); - out_mat (1, 3) = origin.y (); - out_mat (2, 3) = origin.z (); -} - -void checkExtrinics(const sensor_msgs::CameraInfoConstPtr& cinfo_msg){ - - if(!laserReceived || !cameraReceived){ - return; - } - - ROS_INFO("Both received"); - - image_geometry::PinholeCameraModel cam_model_; - cam_model_.fromCameraInfo(cinfo_msg); - - pcl::PointCloud::Ptr trans_cloud(new pcl::PointCloud); - - double x,y,z; - double r,p,yaw; - - tf::StampedTransform transform; - if (listen_to_tf_){ - ROS_INFO("Using available tf transformation"); - tf::TransformListener listener; - - try{ - listener.waitForTransform(target_frame.c_str(), source_frame.c_str(), ros::Time(0), ros::Duration(20.0)); - listener.lookupTransform (target_frame.c_str(), source_frame.c_str(), ros::Time(0), transform); - }catch (tf::TransformException& ex) { - ROS_WARN("TF exception:\n%s", ex.what()); - return; - } - Eigen::Matrix4f transf_mat; - transformAsMatrix(transform, transf_mat); - pcl::transformPointCloud(*edges_cloud, *trans_cloud, transf_mat); - - trans_cloud->header.frame_id = target_frame; - - cv::Mat img_color; - cv::cvtColor(img_out, img_color, cv::COLOR_GRAY2BGR); - - double objective_function = 0; - - ROS_INFO("Accumulating"); - - for (pcl::PointCloud::iterator pt = trans_cloud->points.begin(); pt < trans_cloud->points.end(); pt++) - { - cv::Point3d pt_cv((*pt).x, (*pt).y, (*pt).z); - cv::Point2d uv; - uv = cam_model_.project3dToPixel(pt_cv); - - COLOUR c = GetColour(int((*pt).intensity*255.0), 0, 255); - - cv::circle(img_color, uv, 4, cv::Scalar(int(255*c.b),int(255*c.g),int(255*c.r)), -1); - - if (uv.x>=0 && uv.x < img_out.cols && uv.y >= 0 && uv.y < img_out.rows){ - objective_function += img_out.at(uv.y, uv.x)*(*pt).intensity; - } - - } - - if (save_to_file_){ - savefile << transform.getOrigin()[0] << ", " << transform.getOrigin()[1] << ", " << transform.getOrigin()[2] << ", " << transform.getRotation()[0] << ", " << transform.getRotation()[1] << ", " << transform.getRotation()[2] << ", " << transform.getRotation()[3] << ", " << objective_function << ", " << endl; - } - - ROS_INFO("Objective funcion: %lf", objective_function); - - cv::imshow("Final Levinson", img_color); - cv::waitKey(3); - edges_cloud.reset(); - - laserReceived =false; - cameraReceived = false; - - step_++; - return; - - }else{ - - tf::TransformListener listener; - - try{ - listener.waitForTransform(target_frame.c_str(), source_frame.c_str(), ros::Time(0), ros::Duration(20.0)); - listener.lookupTransform (target_frame.c_str(), source_frame.c_str(), ros::Time(0), transform); - }catch (tf::TransformException& ex) { - ROS_WARN("TF exception:\n%s", ex.what()); - return; - } - - x= transform.getOrigin().getX(); - y= transform.getOrigin().getY(); - z= transform.getOrigin().getZ(); - transform.getBasis().getRPY(r,p,yaw); - - } - - ROS_INFO("%lf %lf %lf %lf %lf %lf", x, y, z, r, p, yaw); - - double x_mod, y_mod, z_mod; - double r_mod, yaw_mod, p_mod; - - int MAX = 5; - int M = (MAX-1)/2; - double INC = 0.001, INC_ANG = 0.001; - - //#pragma omp parallel for - for (int iter_x=0; iter_xheader.frame_id = target_frame; - - cv::Mat img_color; - cv::cvtColor(img_out, img_color, cv::COLOR_GRAY2BGR); - - double objective_function = 0; - - for (pcl::PointCloud::iterator pt = trans_cloud->points.begin(); pt < trans_cloud->points.end(); ++pt) - { - cv::Point3d pt_cv((*pt).x, (*pt).y, (*pt).z); - cv::Point2d uv; - uv = cam_model_.project3dToPixel(pt_cv); - - COLOUR c = GetColour(int((*pt).intensity*255.0), 0, 255); - - cv::circle(img_color, uv, 4, cv::Scalar(int(255*c.b),int(255*c.g),int(255*c.r)), -1); - - if (uv.x>=0 && uv.x < img_out.cols && uv.y >= 0 && uv.y < img_out.rows){ - objective_function += img_out.at(uv.y, uv.x)*(*pt).intensity; - } - - } - - if (save_to_file_){ - savefile << transform.getOrigin()[0] << ", " << transform.getOrigin()[1] << ", " << transform.getOrigin()[2] << ", " << r_mod << ", " << p_mod << ", " << yaw_mod << ", " << objective_function << ", " << endl; - } - - } - } - } - } - } - } - - edges_cloud.reset(); - - laserReceived =false; - cameraReceived = false; - - step_++; - -}; - -void laser_callback(const sensor_msgs::PointCloud2::ConstPtr& velo_cloud){ - ROS_INFO("Velodyne pattern ready!"); - laserReceived = true; - pcl::PointCloud::Ptr t_laser_cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr laser_cloud(new pcl::PointCloud); - fromROSMsg(*velo_cloud, *laser_cloud); - - Velodyne::addRange(*laser_cloud); - - edges_pointcloud(laser_cloud); - - edges_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - - float THRESHOLD = 0.30; - for (pcl::PointCloud::iterator pt = laser_cloud->points.begin(); pt < laser_cloud->points.end(); ++pt){ - if(pow(pt->intensity,2)>THRESHOLD){ - edges_cloud->push_back(*pt); - } - } - - sensor_msgs::PointCloud2 cloud_ros; - pcl::toROSMsg(*edges_cloud, cloud_ros); - cloud_ros.header = velo_cloud->header; - cloud_pub.publish(cloud_ros); - - t_laser_cloud.reset(); - laser_cloud.reset(); - -} - -void stereo_callback(const sensor_msgs::ImageConstPtr& image_msg){ - ROS_INFO("Camera pattern ready!"); - cameraReceived = true; - - cv::Mat img_in; - try{ - img_in = cv_bridge::toCvShare(image_msg)->image; - }catch (cv_bridge::Exception& e){ - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - cv::Mat img_gray; - cv::cvtColor(img_in, img_gray, cv::COLOR_RGB2GRAY); - - cv::Mat img_edge(img_gray.size(), CV_8UC1);; - edge_max(img_gray, img_edge); - - img_out = cv::Mat::zeros(img_edge.size(), CV_8UC1); - inverse_distance_transformation(img_edge, img_out); -} - -// Get current date/time, format is YYYY-MM-DD.HH:mm:ss -const std::string currentDateTime() { - time_t now = time(0); - struct tm tstruct; - char buf[80]; - tstruct = *localtime(&now); - // Visit http://en.cppreference.com/w/cpp/chrono/c/strftime - // for more information about date/time format - strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct); - - return buf; -} - - -int main(int argc, char **argv){ - ros::init(argc, argv, "levinson"); - ros::NodeHandle nh_("~"); // LOCAL - image_transport::ImageTransport it_(nh_); - - // Parameters - nh_.param("target_frame", target_frame, "/stereo_camera"); - nh_.param("source_frame", source_frame, "/velodyne"); - nh_.param("listen_to_tf", listen_to_tf_, true); - nh_.param("save_to_file", save_to_file_, true); - - laserReceived = false; - cameraReceived = false; - - image_transport::Subscriber sub = it_.subscribe("image", 1, stereo_callback); - ros::Subscriber laser_sub = nh_.subscribe ("cloud", 1, laser_callback); - ros::Subscriber cinfo_sub = nh_.subscribe("camera_info", 1, checkExtrinics); - - cloud_pub = nh_.advertise ("levinson_cloud", 1); - - step_ = 0; - - ostringstream os; - os << getenv("HOME") << "/" << "levinson_" << currentDateTime() << ".csv" ; - - if (save_to_file_){ - ROS_INFO("Opening %s", os.str().c_str()); - savefile.open (os.str().c_str()); - - savefile << "x, y, z, r, p, y, obj" << endl; - } - - ros::spin(); - - if (save_to_file_) savefile.close(); - return 0; -} diff --git a/src/lidar_pattern.cpp b/src/lidar_pattern.cpp new file mode 100644 index 0000000..09d6152 --- /dev/null +++ b/src/lidar_pattern.cpp @@ -0,0 +1,570 @@ +/* + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel + + This file is part of velo2cam_calibration. + + velo2cam_calibration 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 2 of the License, or + (at your option) any later version. + + velo2cam_calibration 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 velo2cam_calibration. If not, see . +*/ + +/* + lidar_pattern: Find the circle centers in the lidar cloud +*/ + +#define PCL_NO_PRECOMPILE + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace sensor_msgs; + +ros::Publisher cumulative_pub, centers_pub, pattern_pub, range_pub, coeff_pub, + rotated_pattern_pub; +pcl::PointCloud::Ptr cumulative_cloud; + +// Dynamic parameters +double threshold_; +double passthrough_radius_min_, passthrough_radius_max_, circle_radius_, + centroid_distance_min_, centroid_distance_max_; +double delta_width_circles_, delta_height_circles_; +int rings_count_; +Eigen::Vector3f axis_; +double angle_threshold_; +double cluster_tolerance_; +int clouds_proc_ = 0, clouds_used_ = 0; +int min_centers_found_; +double plane_threshold_; +double gradient_threshold_; +double plane_distance_inliers_; +double circle_threshold_; +double target_radius_tolerance_; +double min_cluster_factor_; +bool skip_warmup_; +bool save_to_file_; +std::ofstream savefile; + +bool WARMUP_DONE = false; + +void callback(const PointCloud2::ConstPtr &laser_cloud) { + if (DEBUG) ROS_INFO("[LiDAR] Processing cloud..."); + + pcl::PointCloud::Ptr velocloud( + new pcl::PointCloud), + velo_filtered(new pcl::PointCloud), + pattern_cloud(new pcl::PointCloud), + edges_cloud(new pcl::PointCloud); + + clouds_proc_++; + + // This cloud is already xyz-filtered + fromROSMsg(*laser_cloud, *velocloud); + + Velodyne::addRange(*velocloud); + + // Range passthrough filter + pcl::PassThrough pass2; + pass2.setInputCloud(velocloud); + pass2.setFilterFieldName("range"); + pass2.setFilterLimits(passthrough_radius_min_, passthrough_radius_max_); + pass2.filter(*velo_filtered); + + // Publishing "range_filtered_velo" cloud (segmented plane) + sensor_msgs::PointCloud2 range_ros; + pcl::toROSMsg(*velo_filtered, range_ros); + range_ros.header = laser_cloud->header; + range_pub.publish(range_ros); + + // Plane segmentation + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + + pcl::SACSegmentation plane_segmentation; + plane_segmentation.setModelType(pcl::SACMODEL_PARALLEL_PLANE); + plane_segmentation.setDistanceThreshold(plane_threshold_); + plane_segmentation.setMethodType(pcl::SAC_RANSAC); + plane_segmentation.setAxis(Eigen::Vector3f(axis_[0], axis_[1], axis_[2])); + plane_segmentation.setEpsAngle(angle_threshold_); + plane_segmentation.setOptimizeCoefficients(true); + plane_segmentation.setMaxIterations(1000); + plane_segmentation.setInputCloud(velo_filtered); + plane_segmentation.segment(*inliers, *coefficients); + + if (inliers->indices.size() == 0) { + // Exit 1: plane not found + ROS_WARN( + "[LiDAR] Could not estimate a planar model for the given dataset."); + return; + } + + // Copy coefficients to proper object for further filtering + Eigen::VectorXf coefficients_v(4); + coefficients_v(0) = coefficients->values[0]; + coefficients_v(1) = coefficients->values[1]; + coefficients_v(2) = coefficients->values[2]; + coefficients_v(3) = coefficients->values[3]; + + // Get edges points by range + vector> rings = + Velodyne::getRings(*velocloud, rings_count_); + for (vector>::iterator ring = rings.begin(); + ring < rings.end(); ++ring) { + Velodyne::Point *prev, *succ; + if (ring->empty()) continue; + + (*ring->begin())->intensity = 0; + (*(ring->end() - 1))->intensity = 0; + for (vector::iterator pt = ring->begin() + 1; + pt < ring->end() - 1; pt++) { + Velodyne::Point *prev = *(pt - 1); + Velodyne::Point *succ = *(pt + 1); + (*pt)->intensity = + max(max(prev->range - (*pt)->range, succ->range - (*pt)->range), 0.f); + } + } + + float THRESHOLD = + gradient_threshold_; // 10 cm between the pattern and the background + for (pcl::PointCloud::iterator pt = + velocloud->points.begin(); + pt < velocloud->points.end(); ++pt) { + if (pt->intensity > THRESHOLD) { + edges_cloud->push_back(*pt); + } + } + + if (edges_cloud->points.size() == 0) { + // Exit 2: pattern edges not found + ROS_WARN("[LiDAR] Could not detect pattern edges."); + return; + } + + // Get points belonging to plane in pattern pointcloud + pcl::SampleConsensusModelPlane::Ptr dit( + new pcl::SampleConsensusModelPlane(edges_cloud)); + std::vector inliers2; + dit->selectWithinDistance(coefficients_v, plane_distance_inliers_, inliers2); + pcl::copyPointCloud(*edges_cloud, inliers2, *pattern_cloud); + + // Velodyne specific info no longer needed for calibration + // so standard PointXYZ is used from now on + pcl::PointCloud::Ptr circles_cloud( + new pcl::PointCloud), + xy_cloud(new pcl::PointCloud), + aux_cloud(new pcl::PointCloud), + auxrotated_cloud(new pcl::PointCloud), + cloud_f(new pcl::PointCloud), + centroid_candidates(new pcl::PointCloud); + + vector> rings2 = + Velodyne::getRings(*pattern_cloud, rings_count_); + + // Conversion from Velodyne::Point to pcl::PointXYZ + for (vector>::iterator ring = rings2.begin(); + ring < rings2.end(); ++ring) { + for (vector::iterator pt = ring->begin(); + pt < ring->end(); ++pt) { + pcl::PointXYZ point; + point.x = (*pt)->x; + point.y = (*pt)->y; + point.z = (*pt)->z; + circles_cloud->push_back(point); + } + } + + // Publishing "pattern_circles" cloud (points belonging to the detected plane) + if (DEBUG) { + sensor_msgs::PointCloud2 velocloud_ros2; + pcl::toROSMsg(*circles_cloud, velocloud_ros2); + velocloud_ros2.header = laser_cloud->header; + pattern_pub.publish(velocloud_ros2); + } + + // Rotate cloud to face pattern plane + Eigen::Vector3f xy_plane_normal_vector, floor_plane_normal_vector; + xy_plane_normal_vector[0] = 0.0; + xy_plane_normal_vector[1] = 0.0; + xy_plane_normal_vector[2] = -1.0; + + floor_plane_normal_vector[0] = coefficients->values[0]; + floor_plane_normal_vector[1] = coefficients->values[1]; + floor_plane_normal_vector[2] = coefficients->values[2]; + + Eigen::Affine3f rotation = + getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector); + pcl::transformPointCloud(*circles_cloud, *xy_cloud, rotation); + + // Publishing "rotated_pattern" cloud (plane transformed to be aligned with + // XY) + if (DEBUG) { + sensor_msgs::PointCloud2 ros_rotated_pattern; + pcl::toROSMsg(*xy_cloud, ros_rotated_pattern); + ros_rotated_pattern.header = laser_cloud->header; + rotated_pattern_pub.publish(ros_rotated_pattern); + } + + // Force pattern points to belong to computed plane + pcl::PointXYZ aux_point; + aux_point.x = 0; + aux_point.y = 0; + aux_point.z = (-coefficients_v(3) / coefficients_v(2)); + aux_cloud->push_back(aux_point); + pcl::transformPointCloud(*aux_cloud, *auxrotated_cloud, rotation); + double zcoord_xyplane = auxrotated_cloud->at(0).z; + + for (pcl::PointCloud::iterator pt = xy_cloud->points.begin(); + pt < xy_cloud->points.end(); ++pt) { + pt->z = zcoord_xyplane; + } + + // RANSAC circle detection + pcl::ModelCoefficients::Ptr coefficients3(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers3(new pcl::PointIndices); + + pcl::SACSegmentation circle_segmentation; + circle_segmentation.setModelType(pcl::SACMODEL_CIRCLE2D); + circle_segmentation.setDistanceThreshold(circle_threshold_); + circle_segmentation.setMethodType(pcl::SAC_RANSAC); + circle_segmentation.setOptimizeCoefficients(true); + circle_segmentation.setMaxIterations(1000); + circle_segmentation.setRadiusLimits( + circle_radius_ - target_radius_tolerance_, + circle_radius_ + target_radius_tolerance_); + + pcl::ExtractIndices extract; + if (DEBUG) + ROS_INFO("[LiDAR] Searching for points in cloud of size %lu", + xy_cloud->points.size()); + while (xy_cloud->points.size() > 3) { + circle_segmentation.setInputCloud(xy_cloud); + circle_segmentation.segment(*inliers3, *coefficients3); + if (inliers3->indices.size() == 0) { + if (DEBUG) + ROS_INFO( + "[LiDAR] Optimized circle segmentation failed, trying unoptimized " + "version"); + circle_segmentation.setOptimizeCoefficients(false); + + circle_segmentation.setInputCloud(xy_cloud); + circle_segmentation.segment(*inliers3, *coefficients3); + + // Reset for next iteration + circle_segmentation.setOptimizeCoefficients(true); + if (inliers3->indices.size() == 0) { + break; + } + } + + // Add center point to cloud + pcl::PointXYZ center; + center.x = *coefficients3->values.begin(); + center.y = *(coefficients3->values.begin() + 1); + center.z = zcoord_xyplane; + + centroid_candidates->push_back(center); + + // Remove inliers from pattern cloud to find next circle + extract.setInputCloud(xy_cloud); + extract.setIndices(inliers3); + extract.setNegative(true); + extract.filter(*cloud_f); + xy_cloud.swap(cloud_f); + + if (DEBUG) + ROS_INFO("[LiDAR] Remaining points in cloud %lu", + xy_cloud->points.size()); + } + + if (centroid_candidates->size() < TARGET_NUM_CIRCLES) { + // Exit 3: all centers not found + ROS_WARN("[LiDAR] Not enough centers: %ld", centroid_candidates->size()); + return; + } + + /** + Geometric consistency check + At this point, circles' center candidates have been computed + (found_centers). Now we need to select the set of 4 candidates that best fit + the calibration target geometry. To that end, the following steps are + followed: 1) Create a cloud with 4 points representing the exact geometry of + the calibration target 2) For each possible set of 4 points: compute + similarity score 3) Rotate back the candidates with the highest score to their + original position in the cloud, and add them to cumulative cloud + **/ + std::vector> groups; + comb(centroid_candidates->size(), TARGET_NUM_CIRCLES, groups); + double groups_scores[groups.size()]; // -1: invalid; 0-1 normalized score + for (int i = 0; i < groups.size(); ++i) { + std::vector candidates; + // Build candidates set + for (int j = 0; j < groups[i].size(); ++j) { + pcl::PointXYZ center; + center.x = centroid_candidates->at(groups[i][j]).x; + center.y = centroid_candidates->at(groups[i][j]).y; + center.z = centroid_candidates->at(groups[i][j]).z; + candidates.push_back(center); + } + + // Compute candidates score + Square square_candidate(candidates, delta_width_circles_, + delta_height_circles_); + groups_scores[i] = square_candidate.is_valid() + ? 1.0 + : -1; // -1 when it's not valid, 1 otherwise + } + + int best_candidate_idx = -1; + double best_candidate_score = -1; + for (int i = 0; i < groups.size(); ++i) { + if (best_candidate_score == 1 && groups_scores[i] == 1) { + // Exit 4: Several candidates fit target's geometry + ROS_ERROR( + "[LiDAR] More than one set of candidates fit target's geometry. " + "Please, make sure your parameters are well set. Exiting callback"); + return; + } + if (groups_scores[i] > best_candidate_score) { + best_candidate_score = groups_scores[i]; + best_candidate_idx = i; + } + } + + if (best_candidate_idx == -1) { + // Exit 5: No candidates fit target's geometry + ROS_WARN( + "[LiDAR] Unable to find a candidate set that matches target's " + "geometry"); + return; + } + + // Build selected centers set + pcl::PointCloud::Ptr rotated_back_cloud( + new pcl::PointCloud()); + for (int j = 0; j < groups[best_candidate_idx].size(); ++j) { + pcl::PointXYZ center_rotated_back = pcl::transformPoint( + centroid_candidates->at(groups[best_candidate_idx][j]), + rotation.inverse()); + center_rotated_back.x = (-coefficients->values[1] * center_rotated_back.y - + coefficients->values[2] * center_rotated_back.z - + coefficients->values[3]) / + coefficients->values[0]; + + rotated_back_cloud->push_back(center_rotated_back); + cumulative_cloud->push_back(center_rotated_back); + } + + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(rotated_back_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + } + + // Publishing "cumulative_cloud" cloud (centers found from the beginning) + if (DEBUG) { + sensor_msgs::PointCloud2 ros_pointcloud; + pcl::toROSMsg(*cumulative_cloud, ros_pointcloud); + ros_pointcloud.header = laser_cloud->header; + cumulative_pub.publish(ros_pointcloud); + } + + xy_cloud.reset(); // Free memory + cloud_f.reset(); // Free memory + + ++clouds_used_; + + // Publishing "plane_model" + pcl_msgs::ModelCoefficients m_coeff; + pcl_conversions::moveFromPCL(*coefficients, m_coeff); + m_coeff.header = laser_cloud->header; + coeff_pub.publish(m_coeff); + + if (DEBUG) + ROS_INFO("[LiDAR] %d/%d frames: %ld pts in cloud", clouds_used_, + clouds_proc_, cumulative_cloud->points.size()); + + // Create cloud for publishing centers + pcl::PointCloud::Ptr centers_cloud( + new pcl::PointCloud); + + // Compute circles centers + if (!WARMUP_DONE) { // Compute clusters from detections in the latest frame + getCenterClusters(cumulative_cloud, centers_cloud, cluster_tolerance_, 1, + 1); + } else { // Use cumulative information from previous frames + getCenterClusters(cumulative_cloud, centers_cloud, cluster_tolerance_, + min_cluster_factor_ * clouds_used_, clouds_used_); + if (centers_cloud->points.size() > TARGET_NUM_CIRCLES) { + getCenterClusters(cumulative_cloud, centers_cloud, cluster_tolerance_, + 3.0 * clouds_used_ / 4.0, clouds_used_); + } + } + + // Exit 6: clustering failed + if (centers_cloud->points.size() == TARGET_NUM_CIRCLES) { + sensor_msgs::PointCloud2 ros2_pointcloud; + pcl::toROSMsg(*centers_cloud, ros2_pointcloud); + ros2_pointcloud.header = laser_cloud->header; + + velo2cam_calibration::ClusterCentroids to_send; + to_send.header = laser_cloud->header; + to_send.cluster_iterations = clouds_used_; + to_send.total_iterations = clouds_proc_; + to_send.cloud = ros2_pointcloud; + + centers_pub.publish(to_send); + if (DEBUG) ROS_INFO("[LiDAR] Pattern centers published"); + + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(centers_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + savefile << cumulative_cloud->width; + } + } + + if (save_to_file_) { + savefile << endl; + } + + // Clear cumulative cloud during warm-up phase + if (!WARMUP_DONE) { + cumulative_cloud->clear(); + clouds_proc_ = 0; + clouds_used_ = 0; + } +} + +void param_callback(velo2cam_calibration::LidarConfig &config, uint32_t level) { + passthrough_radius_min_ = config.passthrough_radius_min; + ROS_INFO("[LiDAR] New passthrough_radius_min_ threshold: %f", + passthrough_radius_min_); + passthrough_radius_max_ = config.passthrough_radius_max; + ROS_INFO("[LiDAR] New passthrough_radius_max_ threshold: %f", + passthrough_radius_max_); + circle_radius_ = config.circle_radius; + ROS_INFO("[LiDAR] New pattern circle radius: %f", circle_radius_); + axis_[0] = config.x; + axis_[1] = config.y; + axis_[2] = config.z; + ROS_INFO("[LiDAR] New normal axis for plane segmentation: %f, %f, %f", + axis_[0], axis_[1], axis_[2]); + angle_threshold_ = config.angle_threshold; + ROS_INFO("[LiDAR] New angle threshold: %f", angle_threshold_); + centroid_distance_min_ = config.centroid_distance_min; + ROS_INFO("[LiDAR] New minimum distance between centroids: %f", + centroid_distance_min_); + centroid_distance_max_ = config.centroid_distance_max; + ROS_INFO("[LiDAR] New maximum distance between centroids: %f", + centroid_distance_max_); +} + +void warmup_callback(const std_msgs::Empty::ConstPtr &msg) { + WARMUP_DONE = !WARMUP_DONE; + if (WARMUP_DONE) { + ROS_INFO("[LiDAR] Warm up done, pattern detection started"); + } else { + ROS_INFO("[LiDAR] Detection stopped. Warm up mode activated"); + } +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "lidar_pattern"); + ros::NodeHandle nh; // GLOBAL + ros::NodeHandle nh_("~"); // LOCAL + ros::Subscriber sub = nh_.subscribe("cloud1", 1, callback); + pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + + range_pub = nh_.advertise("range_filtered_cloud", 1); + if (DEBUG) { + pattern_pub = nh_.advertise("pattern_circles", 1); + rotated_pattern_pub = nh_.advertise("rotated_pattern", 1); + cumulative_pub = nh_.advertise("cumulative_cloud", 1); + } + centers_pub = + nh_.advertise("centers_cloud", 1); + coeff_pub = nh_.advertise("plane_model", 1); + + string csv_name; + + nh.param("delta_width_circles", delta_width_circles_, 0.5); + nh.param("delta_height_circles", delta_height_circles_, 0.4); + nh_.param("plane_threshold", plane_threshold_, 0.1); + nh_.param("gradient_threshold", gradient_threshold_, 0.1); + nh_.param("plane_distance_inliers", plane_distance_inliers_, 0.1); + nh_.param("circle_threshold", circle_threshold_, 0.05); + nh_.param("target_radius_tolerance", target_radius_tolerance_, 0.01); + nh_.param("cluster_tolerance", cluster_tolerance_, 0.05); + nh_.param("min_centers_found", min_centers_found_, TARGET_NUM_CIRCLES); + nh_.param("min_cluster_factor", min_cluster_factor_, 0.5); + nh_.param("rings_count", rings_count_, 64); + nh_.param("skip_warmup", skip_warmup_, false); + nh_.param("save_to_file", save_to_file_, false); + nh_.param("csv_name", csv_name, + "lidar_pattern_" + currentDateTime() + ".csv"); + + cumulative_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType + f; + f = boost::bind(param_callback, _1, _2); + server.setCallback(f); + + ros::Subscriber warmup_sub = + nh.subscribe("warmup_switch", 1, warmup_callback); + + if (skip_warmup_) { + ROS_WARN("Skipping warmup"); + WARMUP_DONE = true; + } + + // Just for statistics + if (save_to_file_) { + ostringstream os; + os << getenv("HOME") << "/v2c_experiments/" << csv_name; + if (save_to_file_) { + if (DEBUG) ROS_INFO("Opening %s", os.str().c_str()); + savefile.open(os.str().c_str()); + savefile << "det1_x, det1_y, det1_z, det2_x, det2_y, det2_z, det3_x, " + "det3_y, det3_z, det4_x, det4_y, det4_z, cent1_x, cent1_y, " + "cent1_z, cent2_x, cent2_y, cent2_z, cent3_x, cent3_y, " + "cent3_z, cent4_x, cent4_y, cent4_z, it" + << endl; + } + } + + ros::spin(); + return 0; +} diff --git a/src/mono_qr_pattern.cpp b/src/mono_qr_pattern.cpp new file mode 100644 index 0000000..9976465 --- /dev/null +++ b/src/mono_qr_pattern.cpp @@ -0,0 +1,599 @@ +/* + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel + + This file is part of velo2cam_calibration. + + velo2cam_calibration 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 2 of the License, or + (at your option) any later version. + + velo2cam_calibration 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 velo2cam_calibration. If not, see . +*/ + +/* + mono_qr_pattern: Find the circle centers in the color image by making use of + the ArUco markers +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace cv; + +pcl::PointCloud::Ptr cumulative_cloud; +cv::Ptr dictionary; +ros::Publisher qr_pub, centers_cloud_pub, cumulative_pub, clusters_pub; + +// ROS params +double marker_size_, delta_width_qr_center_, delta_height_qr_center_; +double delta_width_circles_, delta_height_circles_; +int min_detected_markers_; +int frames_proc_ = 0, frames_used_ = 0; +double cluster_tolerance_; +double min_cluster_factor_; + +bool WARMUP_DONE = false; + +bool skip_warmup_; +bool save_to_file_; +std::ofstream savefile; + +Point2f projectPointDist(cv::Point3f pt_cv, const Mat intrinsics, + const Mat distCoeffs) { + // Project a 3D point taking into account distortion + vector input{pt_cv}; + vector projectedPoints; + projectedPoints.resize( + 1); // TODO: Do it batched? (cv::circle is not batched anyway) + projectPoints(input, Mat::zeros(3, 1, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), + intrinsics, distCoeffs, projectedPoints); + return projectedPoints[0]; +} + +Eigen::Vector3f mean(pcl::PointCloud::Ptr cumulative_cloud) { + double x = 0, y = 0, z = 0; + int npoints = cumulative_cloud->points.size(); + for (pcl::PointCloud::iterator pt = + cumulative_cloud->points.begin(); + pt < cumulative_cloud->points.end(); pt++) { + x += (pt->x) / npoints; + y += (pt->y) / npoints; + z += (pt->z) / npoints; + } + return Eigen::Vector3f(x, y, z); +} + +Eigen::Matrix3f covariance(pcl::PointCloud::Ptr cumulative_cloud, + Eigen::Vector3f means) { + double x = 0, y = 0, z = 0; + int npoints = cumulative_cloud->points.size(); + vector points; + + for (pcl::PointCloud::iterator pt = + cumulative_cloud->points.begin(); + pt < cumulative_cloud->points.end(); pt++) { + Eigen::Vector3f p(pt->x, pt->y, pt->z); + points.push_back(p); + } + + Eigen::Matrix3f covarianceMatrix(3, 3); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) { + covarianceMatrix(i, j) = 0.0; + for (int k = 0; k < npoints; k++) { + covarianceMatrix(i, j) += + (means[i] - points[k][i]) * (means[j] - points[k][j]); + } + covarianceMatrix(i, j) /= npoints - 1; + } + return covarianceMatrix; +} + +void imageCallback(const sensor_msgs::ImageConstPtr &msg, + const sensor_msgs::CameraInfoConstPtr &left_info) { + frames_proc_++; + + cv_bridge::CvImageConstPtr cv_img_ptr; + try { + cv_img_ptr = cv_bridge::toCvShare(msg); + } catch (cv_bridge::Exception &e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + cv::Mat image = cv_img_ptr->image; + cv::Mat imageCopy; + image.copyTo(imageCopy); + sensor_msgs::CameraInfoPtr cinfo(new sensor_msgs::CameraInfo(*left_info)); + image_geometry::PinholeCameraModel cam_model_; + + // TODO Not needed at each frame -> Move it to separate callback + Mat cameraMatrix(3, 3, CV_32F); + // Note that camera matrix is K, not P; both are interchangeable only + // if D is zero + cameraMatrix.at(0, 0) = cinfo->K[0]; + cameraMatrix.at(0, 1) = cinfo->K[1]; + cameraMatrix.at(0, 2) = cinfo->K[2]; + cameraMatrix.at(1, 0) = cinfo->K[3]; + cameraMatrix.at(1, 1) = cinfo->K[4]; + cameraMatrix.at(1, 2) = cinfo->K[5]; + cameraMatrix.at(2, 0) = cinfo->K[6]; + cameraMatrix.at(2, 1) = cinfo->K[7]; + cameraMatrix.at(2, 2) = cinfo->K[8]; + + Mat distCoeffs(1, cinfo->D.size(), CV_32F); + for (int i = 0; i < cinfo->D.size(); i++) + distCoeffs.at(0, i) = cinfo->D[i]; + // TODO End of block to move + + // Create vector of markers corners. 4 markers * 4 corners + // Markers order: + // 0-------1 + // | | + // | C | + // | | + // 3-------2 + + // WARNING: IDs are in different order: + // Marker 0 -> aRuCo ID: 1 + // Marker 1 -> aRuCo ID: 2 + // Marker 2 -> aRuCo ID: 4 + // Marker 3 -> aRuCo ID: 3 + + std::vector> boardCorners; +std: + vector boardCircleCenters; + float width = delta_width_qr_center_; + float height = delta_height_qr_center_; + float circle_width = delta_width_circles_ / 2.; + float circle_height = delta_height_circles_ / 2.; + boardCorners.resize(4); + for (int i = 0; i < 4; ++i) { + int x_qr_center = + (i % 3) == 0 ? -1 : 1; // x distances are substracted for QRs on the + // left, added otherwise + int y_qr_center = + (i < 2) ? 1 : -1; // y distances are added for QRs above target's + // center, substracted otherwise + float x_center = x_qr_center * width; + float y_center = y_qr_center * height; + + cv::Point3f circleCenter3d(x_qr_center * circle_width, + y_qr_center * circle_height, 0); + boardCircleCenters.push_back(circleCenter3d); + for (int j = 0; j < 4; ++j) { + int x_qr = (j % 3) == 0 ? -1 : 1; // x distances are added for QRs 0 and + // 3, substracted otherwise + int y_qr = (j < 2) ? 1 : -1; // y distances are added for QRs 0 and 1, + // substracted otherwise + cv::Point3f pt3d(x_center + x_qr * marker_size_ / 2., + y_center + y_qr * marker_size_ / 2., 0); + boardCorners[i].push_back(pt3d); + } + } + + std::vector boardIds{1, 2, 4, 3}; // IDs order as explained above + cv::Ptr board = + cv::aruco::Board::create(boardCorners, dictionary, boardIds); + + cv::Ptr parameters = + cv::aruco::DetectorParameters::create(); + // set tp use corner refinement for accuracy, values obtained + // for pixel coordinates are more accurate than the neaterst pixel + +#if (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION <= 2) || CV_MAJOR_VERSION < 3 + parameters->doCornerRefinement = true; +#else + parameters->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; +#endif + + // Detect markers + std::vector ids; + std::vector> corners; + cv::aruco::detectMarkers(image, dictionary, corners, ids, parameters); + + // Draw detections if at least one marker detected + if (ids.size() > 0) cv::aruco::drawDetectedMarkers(imageCopy, corners, ids); + + cv::Vec3d rvec(0, 0, 0), tvec(0, 0, 0); // Vectors to store initial guess + + // Compute initial guess as average of individual markers poses + if (ids.size() >= min_detected_markers_ && ids.size() <= TARGET_NUM_CIRCLES) { + // Estimate 3D position of the markers + vector rvecs, tvecs; + Vec3f rvec_sin, rvec_cos; + cv::aruco::estimatePoseSingleMarkers(corners, marker_size_, cameraMatrix, + distCoeffs, rvecs, tvecs); + + // Draw markers' axis and centers in color image (Debug purposes) + for (int i = 0; i < ids.size(); i++) { + double x = tvecs[i][0]; + double y = tvecs[i][1]; + double z = tvecs[i][2]; + + cv::Point3f pt_cv(x, y, z); + cv::Point2f uv; + uv = projectPointDist(pt_cv, cameraMatrix, distCoeffs); + + cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], + tvecs[i], 0.1); + + // Accumulate pose for initial guess + tvec[0] += tvecs[i][0]; + tvec[1] += tvecs[i][1]; + tvec[2] += tvecs[i][2]; + rvec_sin[0] += sin(rvecs[i][0]); + rvec_sin[1] += sin(rvecs[i][1]); + rvec_sin[2] += sin(rvecs[i][2]); + rvec_cos[0] += cos(rvecs[i][0]); + rvec_cos[1] += cos(rvecs[i][1]); + rvec_cos[2] += cos(rvecs[i][2]); + } + + // Compute average pose. Rotation computed as atan2(sin/cos) + tvec = tvec / int(ids.size()); + rvec_sin = rvec_sin / int(ids.size()); // Average sin + rvec_cos = rvec_cos / int(ids.size()); // Average cos + rvec[0] = atan2(rvec_sin[0], rvec_cos[0]); + rvec[1] = atan2(rvec_sin[1], rvec_cos[1]); + rvec[2] = atan2(rvec_sin[2], rvec_cos[2]); + + pcl::PointCloud::Ptr centers_cloud( + new pcl::PointCloud); + pcl::PointCloud::Ptr candidates_cloud( + new pcl::PointCloud); + +// Estimate 3D position of the board using detected markers +#if (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION <= 2) || CV_MAJOR_VERSION < 3 + int valid = cv::aruco::estimatePoseBoard(corners, ids, board, cameraMatrix, + distCoeffs, rvec, tvec); +#else + int valid = cv::aruco::estimatePoseBoard(corners, ids, board, cameraMatrix, + distCoeffs, rvec, tvec, true); +#endif + + cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.2); + + // Build transformation matrix to calibration target axis + cv::Mat R(3, 3, cv::DataType::type); + cv::Rodrigues(rvec, R); + + cv::Mat t = cv::Mat::zeros(3, 1, CV_32F); + t.at(0) = tvec[0]; + t.at(1) = tvec[1]; + t.at(2) = tvec[2]; + + cv::Mat board_transform = cv::Mat::eye(3, 4, CV_32F); + R.copyTo(board_transform.rowRange(0, 3).colRange(0, 3)); + t.copyTo(board_transform.rowRange(0, 3).col(3)); + + // Compute coordintates of circle centers + for (int i = 0; i < boardCircleCenters.size(); ++i) { + cv::Mat mat = cv::Mat::zeros(4, 1, CV_32F); + mat.at(0, 0) = boardCircleCenters[i].x; + mat.at(1, 0) = boardCircleCenters[i].y; + mat.at(2, 0) = boardCircleCenters[i].z; + mat.at(3, 0) = 1.0; + + // Transform center to target coords + cv::Mat mat_qr = board_transform * mat; + cv::Point3f center3d; + center3d.x = mat_qr.at(0, 0); + center3d.y = mat_qr.at(1, 0); + center3d.z = mat_qr.at(2, 0); + + // Draw center (DEBUG) + cv::Point2f uv; + uv = projectPointDist(center3d, cameraMatrix, distCoeffs); + circle(imageCopy, uv, 10, Scalar(0, 255, 0), -1); + + // Add center to list + pcl::PointXYZ qr_center; + qr_center.x = center3d.x; + qr_center.y = center3d.y; + qr_center.z = center3d.z; + candidates_cloud->push_back(qr_center); + } + + /** + NOTE: This is included here in the same way as the rest of the modalities + to avoid obvious misdetections, which sometimes happened in our experiments. + In this modality, it should be impossible to have more than a set of + candidates, but we keep the ability of handling different combinations for + eventual future extensions. + + Geometric consistency check + At this point, circles' center candidates have been computed + (found_centers). Now we need to select the set of 4 candidates that best fit + the calibration target geometry. To that end, the following steps are + followed: 1) Create a cloud with 4 points representing the exact geometry of + the calibration target 2) For each possible set of 4 points: compute + similarity score 3) Rotate back the candidates with the highest score to + their original position in the cloud, and add them to cumulative cloud + **/ + std::vector> groups; + comb(candidates_cloud->size(), TARGET_NUM_CIRCLES, groups); + double groups_scores[groups.size()]; // -1: invalid; 0-1 normalized score + for (int i = 0; i < groups.size(); ++i) { + std::vector candidates; + // Build candidates set + for (int j = 0; j < groups[i].size(); ++j) { + pcl::PointXYZ center; + center.x = candidates_cloud->at(groups[i][j]).x; + center.y = candidates_cloud->at(groups[i][j]).y; + center.z = candidates_cloud->at(groups[i][j]).z; + candidates.push_back(center); + } + + // Compute candidates score + Square square_candidate(candidates, delta_width_circles_, + delta_height_circles_); + groups_scores[i] = square_candidate.is_valid() + ? 1.0 + : -1; // -1 when it's not valid, 1 otherwise + } + + int best_candidate_idx = -1; + double best_candidate_score = -1; + for (int i = 0; i < groups.size(); ++i) { + if (best_candidate_score == 1 && groups_scores[i] == 1) { + // Exit 4: Several candidates fit target's geometry + ROS_ERROR( + "[Mono] More than one set of candidates fit target's geometry. " + "Please, make sure your parameters are well set. Exiting callback"); + return; + } + if (groups_scores[i] > best_candidate_score) { + best_candidate_score = groups_scores[i]; + best_candidate_idx = i; + } + } + + if (best_candidate_idx == -1) { + // Exit: No candidates fit target's geometry + ROS_WARN( + "[Mono] Unable to find a candidate set that matches target's " + "geometry"); + return; + } + + for (int j = 0; j < groups[best_candidate_idx].size(); ++j) { + centers_cloud->push_back( + candidates_cloud->at(groups[best_candidate_idx][j])); + } + + // Add centers to cumulative for further clustering + for (int i = 0; i < centers_cloud->size(); i++) { + cumulative_cloud->push_back(centers_cloud->at(i)); + } + frames_used_++; + if (DEBUG){ + ROS_INFO("[Mono] %d/%d frames: %ld pts in cloud", frames_used_, + frames_proc_, cumulative_cloud->points.size()); + } + + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(centers_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + } + + if (DEBUG) { // Draw centers + for (int i = 0; i < centers_cloud->size(); i++) { + cv::Point3f pt_circle1(centers_cloud->at(i).x, centers_cloud->at(i).y, + centers_cloud->at(i).z); + cv::Point2f uv_circle1; + uv_circle1 = projectPointDist(pt_circle1, cameraMatrix, distCoeffs); + circle(imageCopy, uv_circle1, 2, Scalar(255, 0, 255), -1); + } + } + + // Compute centers clusters + pcl::PointCloud::Ptr clusters_cloud( + new pcl::PointCloud); + if (!WARMUP_DONE) { // Compute clusters from detections in the latest frame + copyPointCloud(*centers_cloud, *clusters_cloud); + } else { // Use cumulative information from previous frames + getCenterClusters(cumulative_cloud, clusters_cloud, cluster_tolerance_, + min_cluster_factor_ * frames_used_, frames_used_); + if (clusters_cloud->points.size() > TARGET_NUM_CIRCLES) { + getCenterClusters(cumulative_cloud, clusters_cloud, cluster_tolerance_, + 3.0 * frames_used_ / 4.0, frames_used_); + } + } + + // Publish pointcloud messages + if (DEBUG) { + sensor_msgs::PointCloud2 ros_pointcloud; + pcl::toROSMsg(*centers_cloud, ros_pointcloud); // circles_cloud + ros_pointcloud.header = msg->header; + qr_pub.publish(ros_pointcloud); + } + + if (clusters_cloud->points.size() == TARGET_NUM_CIRCLES) { + sensor_msgs::PointCloud2 centers_pointcloud; + pcl::toROSMsg(*clusters_cloud, centers_pointcloud); + centers_pointcloud.header = msg->header; + if (DEBUG) { + centers_cloud_pub.publish(centers_pointcloud); + } + + velo2cam_calibration::ClusterCentroids to_send; + to_send.header = msg->header; + to_send.cluster_iterations = frames_used_; + to_send.total_iterations = frames_proc_; + to_send.cloud = centers_pointcloud; + clusters_pub.publish(to_send); + + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(clusters_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + savefile << cumulative_cloud->width; + } + } + + if (DEBUG) { + sensor_msgs::PointCloud2 cumulative_pointcloud; + pcl::toROSMsg(*cumulative_cloud, cumulative_pointcloud); + cumulative_pointcloud.header = msg->header; + cumulative_pub.publish(cumulative_pointcloud); + } + + if (save_to_file_) { + savefile << endl; + } + } else { // Markers found != TARGET_NUM_CIRCLES + ROS_WARN("%lu marker(s) found, %d expected. Skipping frame...", ids.size(), + TARGET_NUM_CIRCLES); + } + + if (DEBUG) { + cv::namedWindow("out", WINDOW_NORMAL); + cv::imshow("out", imageCopy); + cv::waitKey(1); + } + + // Clear cumulative cloud during warm-up phase + if (!WARMUP_DONE) { + cumulative_cloud->clear(); + frames_proc_ = 0; + frames_used_ = 0; + } +} + +void param_callback(velo2cam_calibration::MonocularConfig &config, + uint32_t level) { + marker_size_ = config.marker_size; + ROS_INFO("New marker_size_: %f", marker_size_); + delta_width_qr_center_ = config.delta_width_qr_center; + ROS_INFO("New delta_width_qr_center_: %f", delta_width_qr_center_); + delta_height_qr_center_ = config.delta_height_qr_center; + ROS_INFO("New delta_height_qr_center_: %f", delta_height_qr_center_); +} + +void warmup_callback(const std_msgs::Empty::ConstPtr &msg) { + WARMUP_DONE = !WARMUP_DONE; + if (WARMUP_DONE) { + ROS_INFO("Warm up done, pattern detection started"); + } else { + ROS_INFO("Detection stopped. Warm up mode activated"); + } +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "mono_qr_pattern"); + ros::NodeHandle nh; + ros::NodeHandle nh_("~"); + + // Initialize QR dictionary + dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); + + cumulative_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + + if (DEBUG) { + qr_pub = nh_.advertise("qr_cloud", 1); + centers_cloud_pub = + nh_.advertise("centers_pts_cloud", 1); + cumulative_pub = + nh_.advertise("cumulative_cloud", 1); + } + clusters_pub = + nh_.advertise("centers_cloud", 1); + + string csv_name; + + nh.param("delta_width_circles", delta_width_circles_, 0.5); + nh.param("delta_height_circles", delta_height_circles_, 0.4); + nh_.param("marker_size", marker_size_, 0.20); + nh_.param("delta_width_qr_center_", delta_width_qr_center_, 0.55); + nh_.param("delta_height_qr_center_", delta_height_qr_center_, 0.35); + nh_.param("min_detected_markers", min_detected_markers_, 3); + nh_.param("cluster_tolerance", cluster_tolerance_, 0.05); + nh_.param("min_cluster_factor", min_cluster_factor_, 2.0 / 3.0); + nh_.param("skip_warmup", skip_warmup_, false); + nh_.param("save_to_file", save_to_file_, false); + nh_.param("csv_name", csv_name, "mono_pattern_" + currentDateTime() + ".csv"); + + string image_topic, cinfo_topic; + nh_.param("image_topic", image_topic, + "/stereo_camera/left/image_rect_color"); + nh_.param("cinfo_topic", cinfo_topic, + "/stereo_camera/left/camera_info"); + + message_filters::Subscriber image_sub(nh_, image_topic, + 1); + message_filters::Subscriber cinfo_sub( + nh_, cinfo_topic, 1); + + message_filters::TimeSynchronizer + sync(image_sub, cinfo_sub, 10); + sync.registerCallback(boost::bind(&imageCallback, _1, _2)); + + // ROS param callback + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server< + velo2cam_calibration::MonocularConfig>::CallbackType f; + f = boost::bind(param_callback, _1, _2); + server.setCallback(f); + + ros::Subscriber warmup_sub = + nh.subscribe("warmup_switch", 1, warmup_callback); + + if (skip_warmup_) { + ROS_WARN("Skipping warmup"); + WARMUP_DONE = true; + } + + // Just for statistics + if (save_to_file_) { + ostringstream os; + os << getenv("HOME") << "/v2c_experiments/" << csv_name; + if (save_to_file_) { + if (DEBUG) ROS_INFO("Opening %s", os.str().c_str()); + savefile.open(os.str().c_str()); + savefile << "det1_x, det1_y, det1_z, det2_x, det2_y, det2_z, det3_x, " + "det3_y, det3_z, det4_x, det4_y, det4_z, cent1_x, cent1_y, " + "cent1_z, cent2_x, cent2_y, cent2_z, cent3_x, cent3_y, " + "cent3_z, cent4_x, cent4_y, cent4_z, it" + << endl; + } + } + + ros::spin(); + cv::destroyAllWindows(); + return 0; +} diff --git a/src/pcl_coloring.cpp b/src/pcl_coloring.cpp deleted file mode 100644 index 07f36e8..0000000 --- a/src/pcl_coloring.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel - - This file is part of velo2cam_calibration. - - velo2cam_calibration 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 2 of the License, or - (at your option) any later version. - - velo2cam_calibration 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 velo2cam_calibration. If not, see . -*/ - -/* - pcl_coloring: Color the point cloud using the RGB values from the image -*/ - -#define DEBUG 1 - -#include -#include "ros/package.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace sensor_msgs; - -typedef pcl::PointCloud PointCloudXYZRGB; -ros::Publisher pcl_pub; -string target_frame, source_frame; - -void callback(const PointCloud2::ConstPtr& pcl_msg, const CameraInfoConstPtr& cinfo_msg, const ImageConstPtr& image_msg){ - if(DEBUG) ROS_INFO("\n\nColouring VELODYNE CLOUD!!");; - // Conversion - cv_bridge::CvImageConstPtr cv_img_ptr; - cv::Mat image; - try{ - image = cv_bridge::toCvShare(image_msg, "bgr8")->image; - }catch (cv_bridge::Exception& e){ - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - image_geometry::PinholeCameraModel cam_model_; - cam_model_.fromCameraInfo(cinfo_msg); - - pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); // From ROS Msg - pcl::PointCloud::Ptr trans_cloud(new pcl::PointCloud); // After transformation - PointCloudXYZRGB::Ptr coloured = PointCloudXYZRGB::Ptr(new PointCloudXYZRGB); // For coloring purposes - fromROSMsg(*pcl_msg, *pcl_cloud); - - tf::TransformListener listener; - tf::StampedTransform transform; - if(DEBUG) cout << "FRAME ID "<< pcl_cloud->header.frame_id << endl; - - try{ - listener.waitForTransform(target_frame.c_str(), source_frame.c_str(), ros::Time(0), ros::Duration(20.0)); - listener.lookupTransform (target_frame.c_str(), source_frame.c_str(), ros::Time(0), transform); - }catch (tf::TransformException& ex) { - ROS_WARN("[draw_frames] TF exception:\n%s", ex.what()); - return; - } - pcl_ros::transformPointCloud (*pcl_cloud, *trans_cloud, transform); - trans_cloud->header.frame_id = target_frame; - - pcl::copyPointCloud(*trans_cloud, *coloured); - - for (pcl::PointCloud::iterator pt = coloured->points.begin(); pt < coloured->points.end(); ++pt) - { - cv::Point3d pt_cv((*pt).x, (*pt).y, (*pt).z); - cv::Point2d uv; - uv = cam_model_.project3dToPixel(pt_cv); - - if(uv.x>0 && uv.x < image.cols && uv.y > 0 && uv.y < image.rows){ - // Copy colour to laser pointcloud - (*pt).b = image.at(uv)[0]; - (*pt).g = image.at(uv)[1]; - (*pt).r = image.at(uv)[2]; - } - - } - if(DEBUG) ROS_INFO("Publish coloured PC"); - - // Publish coloured PointCloud - sensor_msgs::PointCloud2 pcl_colour_ros; - pcl::toROSMsg(*coloured, pcl_colour_ros); - pcl_colour_ros.header.stamp = pcl_msg->header.stamp ; - pcl_pub.publish(pcl_colour_ros); -} - -int main(int argc, char **argv){ - ros::init(argc, argv, "pcl_coloring"); - ros::NodeHandle nh_("~"); // LOCAL - // Parameters - nh_.param("target_frame", target_frame, "/stereo_camera"); - nh_.param("source_frame", source_frame, "/velodyne"); - - // Subscribers - message_filters::Subscriber pc_sub(nh_, "pointcloud", 1); - message_filters::Subscriber cinfo_sub(nh_, "camera_info", 1); - message_filters::Subscriber image_sub(nh_, "image", 1); - - pcl_pub = nh_.advertise ("velodyne_coloured", 1); - - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10) - message_filters::Synchronizer sync(MySyncPolicy(10), pc_sub, cinfo_sub, image_sub); - sync.registerCallback(boost::bind(&callback, _1, _2, _3)); - - ros::spin(); - return 0; -} diff --git a/src/pcl_projection.cpp b/src/pcl_projection.cpp deleted file mode 100644 index 7dfd18d..0000000 --- a/src/pcl_projection.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel - - This file is part of velo2cam_calibration. - - velo2cam_calibration 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 2 of the License, or - (at your option) any later version. - - velo2cam_calibration 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 velo2cam_calibration. If not, see . -*/ - -/* - pcl_projection: Project the cloud points to the image -*/ - -#define DEBUG 1 - -#include "velo2cam_utils.h" -#include -#include "ros/package.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace sensor_msgs; - -image_transport::Publisher pub; - -typedef struct { - double r,g,b; -} COLOUR; - -COLOUR // Color map -GetColour(double v,double vmin,double vmax) -{ - COLOUR c = {1.0,1.0,1.0}; // white - double dv; - - if (v < vmin) v = vmin; - if (v > vmax) v = vmax; - dv = vmax - vmin; - - if (v < (vmin + 0.25 * dv)) { - c.r = 0; - c.g = 4 * (v - vmin) / dv; - } else if (v < (vmin + 0.5 * dv)) { - c.r = 0; - c.b = 1 + 4 * (vmin + 0.25 * dv - v) / dv; - } else if (v < (vmin + 0.75 * dv)) { - c.r = 4 * (v - vmin - 0.5 * dv) / dv; - c.b = 0; - } else { - c.g = 1 + 4 * (vmin + 0.75 * dv - v) / dv; - c.b = 0; - } - - return(c); -} - -void // Get the TF as an Eigen matrix -transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat) -{ - double mv[12]; - bt.getBasis ().getOpenGLSubMatrix (mv); - - tf::Vector3 origin = bt.getOrigin (); - - out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; - out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; - out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; - - out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1; - out_mat (0, 3) = origin.x (); - out_mat (1, 3) = origin.y (); - out_mat (2, 3) = origin.z (); -} - -void // Get the camera_info P as an Eigen matrix -projectionAsMatrix (const CameraInfoConstPtr& cinfo_msg, Eigen::Matrix4f &out_mat) -{ - for (int i = 0, k = 0; i < 3; i++) { - for (int j = 0; j < 4; j++, k++){ - out_mat(i, j) = cinfo_msg->P[k]; - } - } - out_mat(3,0) = out_mat(3,1) = out_mat(3,2) = 0; - out_mat(3,3) = 1; -} - -void callback(const PointCloud2::ConstPtr& pcl_msg, const CameraInfoConstPtr& cinfo_msg, const ImageConstPtr& image_msg){ - - - pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); // From ROS Msg - pcl::PointCloud::Ptr filtered_pcl_cloud(new pcl::PointCloud); // Only forward points - pcl::PointCloud::Ptr trans_cloud(new pcl::PointCloud); // After transformation - - tf::TransformListener listener; - tf::StampedTransform transform; - - // Get the image - cv::Mat image; - try{ - image = cv_bridge::toCvShare(image_msg)->image; - }catch (cv_bridge::Exception& e){ - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Get the cloud - fromROSMsg(*pcl_msg, *pcl_cloud); - Velodyne::addRange(*pcl_cloud); - - // Get the TF - if(DEBUG) ROS_INFO("Waiting for TF"); - try{ - listener.waitForTransform(image_msg->header.frame_id.c_str(), pcl_msg->header.frame_id.c_str(), ros::Time(0), ros::Duration(20.0)); - listener.lookupTransform (image_msg->header.frame_id.c_str(), pcl_msg->header.frame_id.c_str(), ros::Time(0), transform); - }catch (tf::TransformException& ex) { - ROS_WARN("[pcl_projection] TF exception:\n%s", ex.what()); - return; - } - if(DEBUG) ROS_INFO("TF available"); - - // Filter points behind image plane (approximation) - pcl::PassThrough pass; - pass.setInputCloud (pcl_cloud); - pass.setFilterFieldName ("x"); - pass.setFilterLimits (0.0, 100.0); - pass.filter (*filtered_pcl_cloud); - - // Move the lidar cloud to the camera frame - Eigen::Matrix4f transf_mat; - transformAsMatrix(transform, transf_mat); - - // Thanks @irecorte for this procedure: - // Get the 4x4 projection matrix - Eigen::Matrix4f projection_matrix; - projectionAsMatrix(cinfo_msg, projection_matrix); - - // Get the array of lidar points - Eigen::MatrixXf points_all = filtered_pcl_cloud->getMatrixXfMap(); - - // Only x,y,z,1 (homogeneous coordinates) - Eigen::MatrixXf points = points_all.topRows(4); - points.row(3) = Eigen::MatrixXf::Constant(1, points_all.cols(), 1); - - // Projection - Eigen::MatrixXf points_img = projection_matrix * transf_mat * points; - points_img.row(0) = points_img.row(0).cwiseQuotient(points_img.row(2)); - points_img.row(1) = points_img.row(1).cwiseQuotient(points_img.row(2)); - - // Draw the points - for (int i = 0; i < points_img.cols(); i++){ - cv::Point2i p(points_img(0,i), points_img(1,i)); - // Only points within the image - if (p.x >= 0 && p.x < cinfo_msg->width && p.y >= 0 && p.y < cinfo_msg->height){ - // Apply colormap to depth - float depth = filtered_pcl_cloud->points[i].range; - // Want it faster? Try depth = transf_points(2, i) - COLOUR c = GetColour(int(depth/20.0*255.0), 0, 255); - cv::circle(image, p, 3, cv::Scalar(int(255*c.b),int(255*c.g),int(255*c.r)), -1); - } - } - - // Publish - pub.publish(cv_bridge::CvImage(image_msg->header, image_msg->encoding, image).toImageMsg()); - // if(DEBUG) ROS_INFO("Done"); -} - -int main(int argc, char **argv){ - ros::init(argc, argv, "pcl_projection"); - ros::NodeHandle nh_("~"); // LOCAL - image_transport::ImageTransport it(nh_); - - // Subscribers - message_filters::Subscriber pc_sub(nh_, "pointcloud", 1); - message_filters::Subscriber cinfo_sub(nh_, "camera_info", 1); - message_filters::Subscriber image_sub(nh_, "image", 1); - - pub = it.advertise("image_with_points", 1); - - typedef message_filters::sync_policies::ApproximateTime SyncPolicy; - message_filters::Synchronizer sync(SyncPolicy(10), pc_sub, cinfo_sub, image_sub); - sync.registerCallback(boost::bind(&callback, _1, _2, _3)); - - ros::spin(); - return 0; -} diff --git a/src/plane.cpp b/src/plane.cpp index d7a6cc0..cb5ce8b 100644 --- a/src/plane.cpp +++ b/src/plane.cpp @@ -1,6 +1,7 @@ /* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel This file is part of velo2cam_calibration. @@ -22,25 +23,20 @@ plane: Find a plane model in the cloud using RANSAC */ -#include -#include -#include -#include -#include +#include #include -#include -#include +#include +#include #include #include -#include #include -#include +#include +#include +#include #include -class SACSegmentator -{ -public: - +class SACSegmentator { + public: ros::NodeHandle nh_; ros::Subscriber cloud_sub; ros::Publisher inliers_pub; @@ -52,26 +48,28 @@ class SACSegmentator Eigen::Vector3f Axis; dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamic_reconfigure::Server::CallbackType + f; - SACSegmentator(): - nh_("~"), threshold_(0.1) - { - cloud_sub = nh_.subscribe("input", 1, &SACSegmentator::cloud_callback, this); - inliers_pub = nh_.advertise ("inliers", 1); - coeff_pub = nh_.advertise ("model", 1); + SACSegmentator() : nh_("~"), threshold_(0.1) { + cloud_sub = nh_.subscribe( + "input", 1, &SACSegmentator::cloud_callback, this); + inliers_pub = nh_.advertise("inliers", 1); + coeff_pub = nh_.advertise("model", 1); std::vector axis_param; Axis = Eigen::Vector3f::Zero(); nh_.getParam("axis", axis_param); - if (axis_param.size()==3){ - for (int i=0; i<3; ++i){ + if (axis_param.size() == 3) { + for (int i = 0; i < 3; ++i) { Axis[i] = axis_param[i]; } - }else{ - Axis[0]=0.0; Axis[1]=1.0; Axis[2]=0.0; + } else { + Axis[0] = 0.0; + Axis[1] = 1.0; + Axis[2] = 0.0; } nh_.param("eps_angle", eps_angle_, 0.35); @@ -80,47 +78,49 @@ class SACSegmentator // 1: SACMODEL_PARALLEL_PLANE nh_.param("segmentation_type", sac_segmentation_type_, 0); - if (sac_segmentation_type_ == 0){ - ROS_INFO("Searching for planes perpendicular to %f %f %f", Axis[0], Axis[1], Axis[2]); - }else{ - ROS_INFO("Searching for planes parallel to %f %f %f", Axis[0], Axis[1], Axis[2]); + if (sac_segmentation_type_ == 0) { + ROS_INFO("Searching for planes perpendicular to %f %f %f", Axis[0], + Axis[1], Axis[2]); + } else { + ROS_INFO("Searching for planes parallel to %f %f %f", Axis[0], Axis[1], + Axis[2]); } f = boost::bind(&SACSegmentator::param_callback, this, _1, _2); server.setCallback(f); ROS_INFO("Segmentator ready"); - } - void cloud_callback(const sensor_msgs::PointCloud2::ConstPtr &input) - { - pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); - pcl::fromROSMsg(*input,*cloud); + void cloud_callback(const sensor_msgs::PointCloud2::ConstPtr &input) { + pcl::PointCloud::Ptr cloud( + new pcl::PointCloud()); + pcl::fromROSMsg(*input, *cloud); - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation seg; - seg.setModelType (sac_segmentation_type_ ? pcl::SACMODEL_PARALLEL_PLANE : pcl::SACMODEL_PERPENDICULAR_PLANE); - seg.setDistanceThreshold (threshold_); - seg.setMethodType (pcl::SAC_RANSAC); + seg.setModelType(sac_segmentation_type_ + ? pcl::SACMODEL_PARALLEL_PLANE + : pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setDistanceThreshold(threshold_); + seg.setMethodType(pcl::SAC_RANSAC); seg.setAxis(Axis); - seg.setOptimizeCoefficients (true); + seg.setOptimizeCoefficients(true); seg.setEpsAngle(eps_angle_); seg.setMaxIterations(250); std::vector indices; - pcl::removeNaNFromPointCloud (*cloud, *cloud, indices); + pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); - seg.setInputCloud (cloud->makeShared()); - seg.segment (*inliers, *coefficients); + seg.setInputCloud(cloud->makeShared()); + seg.segment(*inliers, *coefficients); - if (inliers->indices.size () == 0) - { - ROS_WARN ("Could not estimate a planar model for the given dataset."); + if (inliers->indices.size() == 0) { + ROS_WARN("Could not estimate a planar model for the given dataset."); return; } @@ -138,18 +138,17 @@ class SACSegmentator coeff_pub.publish(m_coeff); } - void param_callback(velo2cam_calibration::PlaneConfig &config, uint32_t level){ + void param_callback(velo2cam_calibration::PlaneConfig &config, + uint32_t level) { threshold_ = config.threshold; ROS_INFO("New distance threshold: %f", threshold_); } }; -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "plane_segmentation"); SACSegmentator seg; ros::spin(); - } diff --git a/src/stereo_pattern.cpp b/src/stereo_pattern.cpp index 610ec5d..ffe37f9 100644 --- a/src/stereo_pattern.cpp +++ b/src/stereo_pattern.cpp @@ -1,6 +1,7 @@ /* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel This file is part of velo2cam_calibration. @@ -22,220 +23,101 @@ stereo_pattern: Find the circle centers in the stereo cloud */ -#include -#include -#include -#include +#define TARGET_NUM_CIRCLES 4 + +#include #include -#include #include -#include -#include -#include -#include +#include +#include #include -#include #include -#include -#include -#include - -#include -#include "velo2cam_utils.h" +#include +#include +#include +#include +#include +#include +#include #include +#include using namespace std; using namespace sensor_msgs; -int nFrames; -int images_proc_=0, images_used_=0; +int images_proc_ = 0, images_used_ = 0; +double delta_width_circles_, delta_height_circles_; double circle_threshold_; -double line_threshold_; -double min_plane_normal_z_; double plane_distance_inliers_; -double min_border_x_; -double min_distance_between_borders_x_; -double min_distance_between_borders_y_; -double border_distance_inliers_; -int min_line_inliers_; -double cluster_size_; +double target_radius_tolerance_; +double cluster_tolerance_; +double min_cluster_factor_; int min_centers_found_; +bool WARMUP_DONE = false; +bool skip_warmup_; +bool save_to_file_; +std::ofstream savefile; ros::Publisher inliers_pub; ros::Publisher coeff_pub; -ros::Publisher boundary_edges_pub; -ros::Publisher occluding_edges_pub; -ros::Publisher occluded_edges_pub; -ros::Publisher high_curvature_edges_pub; -ros::Publisher rgb_edges_pub; ros::Publisher plane_edges_pub; -ros::Publisher circles_pub; ros::Publisher xy_pattern_pub; -ros::Publisher no_circles_pub; ros::Publisher cumulative_pub; ros::Publisher final_pub; -ros::Publisher transf_pub; -ros::Publisher auxpoint_pub; pcl::PointCloud::Ptr cumulative_cloud; std_msgs::Header header_; -void publish_cloud(pcl::PointCloud::Ptr input_cloud, ros::Publisher pub){ - sensor_msgs::PointCloud2 ros_cloud; - pcl::toROSMsg(*input_cloud, ros_cloud); - ros_cloud.header = header_; - pub.publish(ros_cloud); -} - -void callback(const PointCloud2::ConstPtr& camera_cloud, - const pcl_msgs::ModelCoefficients::ConstPtr& cam_plane_coeffs){ - - if(DEBUG) ROS_INFO("[Camera] Processing image..."); +void callback(const PointCloud2::ConstPtr &camera_cloud, + const pcl_msgs::ModelCoefficients::ConstPtr &cam_plane_coeffs) { + if (DEBUG) ROS_INFO("[Stereo] Processing image..."); images_proc_++; header_ = camera_cloud->header; - pcl::PointCloud::Ptr cam_cloud (new pcl::PointCloud ()); + pcl::PointCloud::Ptr cam_cloud( + new pcl::PointCloud()); - pcl::fromROSMsg(*camera_cloud,*cam_cloud); + pcl::fromROSMsg(*camera_cloud, *cam_cloud); - // 1.FILTER THE ONLY-EDGES-CLOUD ACCORDING TO THE DETECTED PLANE - // Make sure that the plane makes sense - if (cam_plane_coeffs->values[2]values[0]; coefficients_v(1) = cam_plane_coeffs->values[1]; coefficients_v(2) = cam_plane_coeffs->values[2]; coefficients_v(3) = cam_plane_coeffs->values[3]; - pcl::PointCloud::Ptr cam_plane_cloud (new pcl::PointCloud ()); - pcl::SampleConsensusModelPlane::Ptr dit (new pcl::SampleConsensusModelPlane (cam_cloud)); + pcl::PointCloud::Ptr cam_plane_cloud( + new pcl::PointCloud()); + pcl::SampleConsensusModelPlane::Ptr dit( + new pcl::SampleConsensusModelPlane(cam_cloud)); std::vector plane_inliers; - dit->selectWithinDistance (coefficients_v, plane_distance_inliers_, plane_inliers); - pcl::copyPointCloud(*cam_cloud, plane_inliers, *cam_plane_cloud); + dit->selectWithinDistance(coefficients_v, plane_distance_inliers_, + plane_inliers); + pcl::copyPointCloud(*cam_cloud, plane_inliers, + *cam_plane_cloud); // Publish plane as "plane_edges_cloud" - PointCloud2 plane_edges_ros; - pcl::toROSMsg(*cam_plane_cloud, plane_edges_ros); - plane_edges_ros.header = camera_cloud->header; - plane_edges_pub.publish(plane_edges_ros); - - // 2.REMOVE BORDER LINES - // Find them - pcl::PointCloud::Ptr temp_line_cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); - pcl::SACSegmentation lineseg; - pcl::ExtractIndices extract; - std::vector out_lines; - - lineseg.setModelType (pcl::SACMODEL_LINE); - lineseg.setDistanceThreshold (line_threshold_); - lineseg.setMethodType (pcl::SAC_RANSAC); - lineseg.setOptimizeCoefficients (true); - lineseg.setMaxIterations(1000); - - pcl::copyPointCloud(*cam_plane_cloud, *temp_line_cloud); - - float first_line_x = -1.0, first_line_y = -1.0; - int last_inliers; - - do{ - lineseg.setInputCloud (temp_line_cloud); - lineseg.segment (*inliers, *coefficients); - if (inliers->indices.size () == 0) - { - if (DEBUG) ROS_INFO("Could not estimate a line model for the given dataset."); - break; - } - - last_inliers = inliers->indices.size (); - - if (inliers->indices.size () < min_line_inliers_){ - continue; - } - - // Extract the inliers - extract.setInputCloud (temp_line_cloud); - extract.setIndices (inliers); - - // Remove inliers from cloud to find next line - extract.setNegative (true); - extract.filter(*temp_cloud); - temp_line_cloud.swap(temp_cloud); - - //if(DEBUG) ROS_INFO("Line orientation %f %f %f", coefficients->values[3], coefficients->values[4], coefficients->values[5 ]); - - if (fabs(coefficients->values[3])values[4])values[3]) >= min_border_x_){ - if (first_line_x<0.0) first_line_x = coefficients->values[0]; - else if (fabs(coefficients->values[0]-first_line_x) < min_distance_between_borders_x_){ - //if(DEBUG) ROS_INFO("Invalid line (proximity)"); - continue; - } - }else if(fabs(coefficients->values[4]) >= min_border_x_){ - if (first_line_y<0.0) first_line_y = coefficients->values[1]; - else if (fabs(coefficients->values[1]-first_line_y) < min_distance_between_borders_y_){ - //if(DEBUG) ROS_INFO("Invalid line (proximity)"); - continue; - } - } - - if(DEBUG) ROS_INFO("[Camera] Removed line: %d inliers / orientation: %f %f %f", - last_inliers, coefficients->values[3], coefficients->values[4], - coefficients->values[5]); - - out_lines.push_back(*coefficients); - - }while (last_inliers > min_line_inliers_ && out_lines.size() < 4); - - pcl::PointCloud::Ptr cam_plane_wol_cloud (new pcl::PointCloud ()); - - // Remove them - for (vector::iterator it=out_lines.begin(); it::Ptr dil (new pcl::SampleConsensusModelLine (cam_plane_cloud)); - Eigen::VectorXf coefficients_l(6); - for(int j=0; j<6; j++){ - coefficients_l(j) = it->values[j]; - } - std::vector line_inliers; - dil->selectWithinDistance (coefficients_l, border_distance_inliers_, line_inliers); - - line_ind->indices.resize(line_inliers.size()); - - std::copy(line_inliers.begin(), line_inliers.end(), line_ind->indices.begin()); - - // Extract the inliers - extract.setInputCloud (cam_plane_cloud); - extract.setIndices (line_ind); - extract.setNegative (true); - extract.filter (*cam_plane_wol_cloud); - - cam_plane_wol_cloud.swap(cam_plane_cloud); + if (DEBUG) { + PointCloud2 plane_edges_ros; + pcl::toROSMsg(*cam_plane_cloud, plane_edges_ros); + plane_edges_ros.header = camera_cloud->header; + plane_edges_pub.publish(plane_edges_ros); } - PointCloud2 circles_ros; - pcl::toROSMsg(*cam_plane_cloud, circles_ros); - circles_ros.header = camera_cloud->header; - circles_pub.publish(circles_ros); + pcl::PointCloud::Ptr temp_cloud( + new pcl::PointCloud); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ExtractIndices extract; - // 3.ROTATE CLOUD TO FACE PATTERN PLANE - pcl::PointCloud::Ptr xy_cloud(new pcl::PointCloud); + // 2.ROTATE CLOUD TO FACE PATTERN PLANE + pcl::PointCloud::Ptr xy_cloud( + new pcl::PointCloud); Eigen::Vector3f xy_plane_normal_vector, floor_plane_normal_vector; xy_plane_normal_vector[0] = 0.0; xy_plane_normal_vector[1] = 0.0; @@ -246,123 +128,195 @@ void callback(const PointCloud2::ConstPtr& camera_cloud, floor_plane_normal_vector[2] = cam_plane_coeffs->values[2]; std::vector indices; - pcl::removeNaNFromPointCloud (*cam_plane_cloud, *cam_plane_cloud, indices); + pcl::removeNaNFromPointCloud(*cam_plane_cloud, *cam_plane_cloud, indices); - Eigen::Affine3f rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector); + Eigen::Affine3f rotation = + getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector); pcl::transformPointCloud(*cam_plane_cloud, *xy_cloud, rotation); - pcl::PointCloud::Ptr aux_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr aux_cloud( + new pcl::PointCloud); pcl::PointXYZ aux_point; aux_point.x = 0; aux_point.y = 0; - aux_point.z = (-coefficients_v(3)/coefficients_v(2)); + aux_point.z = (-coefficients_v(3) / coefficients_v(2)); aux_cloud->push_back(aux_point); - pcl::PointCloud::Ptr auxrotated_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr auxrotated_cloud( + new pcl::PointCloud); pcl::transformPointCloud(*aux_cloud, *auxrotated_cloud, rotation); - sensor_msgs::PointCloud2 ros_auxpoint; - pcl::toROSMsg(*aux_cloud, ros_auxpoint); - ros_auxpoint.header = camera_cloud->header; - auxpoint_pub.publish(ros_auxpoint); - double zcoord_xyplane = auxrotated_cloud->at(0).z; // Force pattern points to belong to computed plane for (pcl::PointCloud::iterator pt = xy_cloud->points.begin(); - pt < xy_cloud->points.end(); ++pt){ + pt < xy_cloud->points.end(); ++pt) { pt->z = zcoord_xyplane; } - PointCloud2 xy_pattern_ros; - pcl::toROSMsg(*xy_cloud, xy_pattern_ros); - xy_pattern_ros.header = camera_cloud->header; - xy_pattern_pub.publish(xy_pattern_ros); + // Publishing "xy_pattern" (pattern transformed to be aligned with XY) + if (DEBUG) { + PointCloud2 xy_pattern_ros; + pcl::toROSMsg(*xy_cloud, xy_pattern_ros); + xy_pattern_ros.header = camera_cloud->header; + xy_pattern_pub.publish(xy_pattern_ros); + } - // 4.FIND CIRCLES + // 3.FIND CIRCLES pcl::SACSegmentation seg; - seg.setModelType (pcl::SACMODEL_CIRCLE2D); - seg.setDistanceThreshold (circle_threshold_); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setOptimizeCoefficients (true); + seg.setModelType(pcl::SACMODEL_CIRCLE2D); + seg.setDistanceThreshold(circle_threshold_); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setOptimizeCoefficients(true); seg.setMaxIterations(1000); - seg.setRadiusLimits(0.11,0.13); - - pcl::PointCloud::Ptr circle_cloud(new pcl::PointCloud); - - std::vector< std::vector > found_centers; - bool valid = true; - - do{ - - seg.setInputCloud (xy_cloud); - seg.segment (*inliers, *coefficients); - if (inliers->indices.size () == 0) - { - ROS_WARN ("[Camera] Could not estimate a circle model for the given dataset."); + seg.setRadiusLimits(TARGET_RADIUS - target_radius_tolerance_, + TARGET_RADIUS + target_radius_tolerance_); + + pcl::PointCloud::Ptr circle_cloud( + new pcl::PointCloud); + + std::vector> found_centers; + + do { + seg.setInputCloud(xy_cloud); + seg.segment(*inliers, *coefficients); + if (inliers->indices.size() == 0) { + if (found_centers.size() < 1) { + ROS_WARN( + "[Stereo] Could not estimate a circle model for the given " + "dataset."); + } break; - }else{ - if(DEBUG) ROS_INFO ("[Camera] Found circle: %lu inliers", inliers->indices.size ()); + } else { + if (DEBUG) + ROS_INFO("[Stereo] Found circle: %lu inliers", inliers->indices.size()); } // Extract the inliers - extract.setInputCloud (xy_cloud); - extract.setIndices (inliers); - extract.setNegative (false); - extract.filter (*circle_cloud); + extract.setInputCloud(xy_cloud); + extract.setIndices(inliers); + extract.setNegative(false); + extract.filter(*circle_cloud); - // Add center point to cloud + // Add center point to cloud (only if it makes sense) pcl::PointXYZ center; center.x = *coefficients->values.begin(); - center.y = *(coefficients->values.begin()+1); - for(std::vector >::iterator it = found_centers.begin(); it != found_centers.end(); ++it) { - if (sqrt(pow(fabs((*it)[0]-center.x),2) + pow(fabs((*it)[1]-center.y),2))<0.25){ - valid = false; - } - } - + center.y = *(coefficients->values.begin() + 1); center.z = zcoord_xyplane; - if (valid){ - std::vector found_center; - found_center.push_back(center.x); - found_center.push_back(center.y); - found_center.push_back(center.z); - found_centers.push_back(found_center); - } + std::vector found_center; + found_center.push_back(center.x); + found_center.push_back(center.y); + found_center.push_back(center.z); + found_centers.push_back(found_center); // Remove inliers from pattern cloud to find next circle - extract.setNegative (true); + extract.setNegative(true); extract.filter(*temp_cloud); xy_cloud.swap(temp_cloud); - PointCloud2 no_circles_ros; - pcl::toROSMsg(*xy_cloud, no_circles_ros); - no_circles_ros.header = camera_cloud->header; - no_circles_pub.publish(no_circles_ros); - - }while(found_centers.size() < 4 && valid); + } while (xy_cloud->points.size() > 3); - if (found_centers.size() < min_centers_found_ || found_centers.size() > 4){ + if (found_centers.size() < + min_centers_found_) { // Usually min_centers_found_ = TARGET_NUM_CIRCLES + // Exit 1: centers not found ROS_WARN("Not enough centers: %ld", found_centers.size()); return; - }else{ - for (std::vector >::iterator it = found_centers.begin(); it < found_centers.end(); ++it){ + } + + /** + 4. GEOMETRIC CONSISTENCY CHECK + At this point, circles' center candidates have been computed + (found_centers). Now we need to select the set of 4 candidates that best fit + the calibration target geometry. To that end, the following steps are + followed: 1) Create a cloud with 4 points representing the exact geometry of + the calibration target 2) For each possible set of 4 points: compute + similarity score 3) Rotate back the candidates with the highest score to their + original position in the cloud, and add them to cumulative cloud + **/ + std::vector> groups; + comb(found_centers.size(), TARGET_NUM_CIRCLES, groups); + double groups_scores[groups.size()]; // -1: invalid; 0-1 normalized score + for (int i = 0; i < groups.size(); ++i) { + std::vector candidates; + // Build candidates set + for (int j = 0; j < groups[i].size(); ++j) { pcl::PointXYZ center; - center.x = (*it)[0]; - center.y = (*it)[1]; - center.z = (*it)[2];; - pcl::PointXYZ center_rotated_back = pcl::transformPoint(center, rotation.inverse()); - center_rotated_back.z = (-cam_plane_coeffs->values[0]*center_rotated_back.x - cam_plane_coeffs->values[1]*center_rotated_back.y - cam_plane_coeffs->values[3])/cam_plane_coeffs->values[2]; - cumulative_cloud->push_back(center_rotated_back); + center.x = found_centers[groups[i][j]][0]; + center.y = found_centers[groups[i][j]][1]; + center.z = found_centers[groups[i][j]][2]; + candidates.push_back(center); } + + // Compute candidates score + Square square_candidate(candidates, delta_width_circles_, + delta_height_circles_); + groups_scores[i] = square_candidate.is_valid() + ? 1.0 + : -1; // -1 when it's not valid, 1 otherwise } - PointCloud2 cum_ros; - pcl::toROSMsg(*cumulative_cloud, cum_ros); - cum_ros.header = camera_cloud->header; - cumulative_pub.publish(cum_ros); + int best_candidate_idx = -1; + double best_candidate_score = -1; + for (int i = 0; i < groups.size(); ++i) { + if (best_candidate_score == 1 && groups_scores[i] == 1) { + // Exit 2: Several candidates fit target's geometry + ROS_ERROR( + "[Stereo] More than one set of candidates fit target's geometry. " + "Please, make sure your parameters are well set. Exiting callback"); + return; + } + if (groups_scores[i] > best_candidate_score) { + best_candidate_score = groups_scores[i]; + best_candidate_idx = i; + } + } + + if (best_candidate_idx == -1) { + // Exit 3: No candidates fit target's geometry + ROS_WARN( + "[Stereo] Unable to find a candidate set that matches target's " + "geometry"); + return; + } + + pcl::PointCloud::Ptr rotated_back_cloud( + new pcl::PointCloud()); + for (int j = 0; j < groups[best_candidate_idx].size(); ++j) { + pcl::PointXYZ point; + point.x = found_centers[groups[best_candidate_idx][j]][0]; + point.y = found_centers[groups[best_candidate_idx][j]][1]; + point.z = found_centers[groups[best_candidate_idx][j]][2]; + + pcl::PointXYZ center_rotated_back = + pcl::transformPoint(point, rotation.inverse()); + center_rotated_back.z = + (-cam_plane_coeffs->values[0] * center_rotated_back.x - + cam_plane_coeffs->values[1] * center_rotated_back.y - + cam_plane_coeffs->values[3]) / + cam_plane_coeffs->values[2]; + + rotated_back_cloud->push_back(center_rotated_back); + cumulative_cloud->push_back(center_rotated_back); + } + + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(rotated_back_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + } + + // Publishing "cumulative_cloud" (centers found from the beginning) + if (DEBUG) { + PointCloud2 cumulative_ros; + pcl::toROSMsg(*cumulative_cloud, cumulative_ros); + cumulative_ros.header = camera_cloud->header; + cumulative_pub.publish(cumulative_ros); + } pcl_msgs::PointIndices p_ind; @@ -374,21 +328,42 @@ void callback(const PointCloud2::ConstPtr& camera_cloud, pcl_conversions::moveFromPCL(*coefficients, m_coeff); m_coeff.header = camera_cloud->header; - inliers_pub.publish(p_ind); - coeff_pub.publish(m_coeff); - - nFrames++; - images_used_ = nFrames; - ROS_INFO("[Camera] %d/%d frames: %ld pts in cloud", - images_used_, images_proc_, cumulative_cloud->points.size()); - pcl::PointCloud::Ptr final_cloud(new pcl::PointCloud); + if (DEBUG) { + inliers_pub.publish(p_ind); + coeff_pub.publish(m_coeff); + } - getCenterClusters(cumulative_cloud, final_cloud, 0.1, nFrames/2, nFrames); - if (final_cloud->points.size()>4){ - getCenterClusters(cumulative_cloud, final_cloud, 0.1, 3.0*nFrames/4.0, nFrames); + images_used_++; + if (DEBUG) { + ROS_INFO("[Stereo] %d/%d frames: %ld pts in cloud", images_used_, + images_proc_, cumulative_cloud->points.size()); + } + pcl::PointCloud::Ptr final_cloud( + new pcl::PointCloud); + + // Compute circles centers + if (!WARMUP_DONE) { // Compute clusters from detections in the latest frame + getCenterClusters(cumulative_cloud, final_cloud, cluster_tolerance_, 1, 1); + } else { // Use cumulative information from previous frames + getCenterClusters(cumulative_cloud, final_cloud, cluster_tolerance_, + min_cluster_factor_ * images_used_, images_used_); + if (final_cloud->points.size() > TARGET_NUM_CIRCLES) { + getCenterClusters(cumulative_cloud, final_cloud, cluster_tolerance_, + 3.0 * images_used_ / 4.0, images_used_); + } } - if (final_cloud->points.size()==4){ + // Exit 4: clustering failed + if (final_cloud->points.size() == TARGET_NUM_CIRCLES) { + if (save_to_file_) { + std::vector sorted_centers; + sortPatternCenters(final_cloud, sorted_centers); + for (std::vector::iterator it = sorted_centers.begin(); + it < sorted_centers.end(); ++it) { + savefile << it->x << ", " << it->y << ", " << it->z << ", "; + } + savefile << cumulative_cloud->width; + } sensor_msgs::PointCloud2 final_ros; pcl::toROSMsg(*final_cloud, final_ros); @@ -402,57 +377,110 @@ void callback(const PointCloud2::ConstPtr& camera_cloud, final_pub.publish(to_send); } + + if (save_to_file_) { + savefile << endl; + } + + // Clear cumulative cloud during warm-up phase + if (!WARMUP_DONE) { + cumulative_cloud->clear(); + images_proc_ = 0; + images_used_ = 0; + } } -void param_callback(velo2cam_calibration::CameraConfig &config, uint32_t level){ +void param_callback(velo2cam_calibration::StereoConfig &config, + uint32_t level) { circle_threshold_ = config.circle_threshold; - ROS_INFO("New circle threshold: %f", circle_threshold_); - line_threshold_ = config.line_threshold; - ROS_INFO("New line threshold: %f", line_threshold_); + ROS_INFO("[Stereo] New circle threshold: %f", circle_threshold_); } -int main(int argc, char **argv){ +void warmup_callback(const std_msgs::Empty::ConstPtr &msg) { + WARMUP_DONE = !WARMUP_DONE; + if (WARMUP_DONE) { + ROS_INFO("[Stereo] Warm up done, pattern detection started"); + } else { + ROS_INFO("[Stereo] Detection stopped. Warm up mode activated"); + } +} + +int main(int argc, char **argv) { ros::init(argc, argv, "stereo_pattern"); - ros::NodeHandle nh_("~"); + ros::NodeHandle nh; // GLOBAL + ros::NodeHandle nh_("~"); // LOCAL message_filters::Subscriber camera_cloud_sub_; - message_filters::Subscriber cam_plane_coeffs_sub_; + message_filters::Subscriber + cam_plane_coeffs_sub_; camera_cloud_sub_.subscribe(nh_, "cloud2", 1); cam_plane_coeffs_sub_.subscribe(nh_, "cam_plane_coeffs", 1); - inliers_pub = nh_.advertise ("inliers", 1); - coeff_pub = nh_.advertise ("model", 1); - plane_edges_pub = nh_.advertise ("plane_edges_cloud", 1); - circles_pub = nh_.advertise ("circles_cloud", 1); - xy_pattern_pub = nh_.advertise ("xy_pattern", 1); - no_circles_pub = nh_.advertise ("no_circles", 1); - cumulative_pub = nh_.advertise ("cumulative_cloud", 1); - final_pub = nh_.advertise ("centers_cloud", 1); - transf_pub = nh_.advertise ("transf_cam", 1); - auxpoint_pub= nh_.advertise ("aux_point", 1); - - cumulative_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - - typedef message_filters::sync_policies::ExactTime MySyncPolicy; - message_filters::Synchronizer sync_(MySyncPolicy(10), camera_cloud_sub_, cam_plane_coeffs_sub_); + if (DEBUG) { + inliers_pub = nh_.advertise("inliers", 1); + coeff_pub = nh_.advertise("model", 1); + plane_edges_pub = nh_.advertise("plane_edges_cloud", 1); + xy_pattern_pub = nh_.advertise("xy_pattern", 1); + cumulative_pub = nh_.advertise("cumulative_cloud", 1); + } + + final_pub = + nh_.advertise("centers_cloud", 1); + + cumulative_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + + typedef message_filters::sync_policies::ExactTime + ExSync; + message_filters::Synchronizer sync_(ExSync(10), camera_cloud_sub_, + cam_plane_coeffs_sub_); sync_.registerCallback(boost::bind(&callback, _1, _2)); - nh_.param("min_plane_normal_z", min_plane_normal_z_, 0.96); - nh_.param("plane_distance_inliers", plane_distance_inliers_, 0.01); - nh_.param("min_border_x", min_border_x_, 0.96); - nh_.param("min_distance_between_borders_x", min_distance_between_borders_x_, 1.0); - nh_.param("min_distance_between_borders_y", min_distance_between_borders_y_, 0.6); - nh_.param("border_distance_inliers", border_distance_inliers_, 0.05); - nh_.param("min_line_inliers", min_line_inliers_, 1200); //TODO: Adapt to the distance to the plane - nh_.param("cluster_size", cluster_size_, 0.02); - nh_.param("min_centers_found", min_centers_found_, 4); - - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + string csv_name; + + nh.param("delta_width_circles", delta_width_circles_, 0.5); + nh.param("delta_height_circles", delta_height_circles_, 0.4); + nh_.param("plane_distance_inliers", plane_distance_inliers_, 0.1); + nh_.param("target_radius_tolerance", target_radius_tolerance_, 0.01); + nh_.param("min_centers_found", min_centers_found_, TARGET_NUM_CIRCLES); + nh_.param("cluster_tolerance", cluster_tolerance_, 0.05); + nh_.param("min_cluster_factor", min_cluster_factor_, 0.5); + nh_.param("skip_warmup", skip_warmup_, false); + nh_.param("save_to_file", save_to_file_, false); + nh_.param("csv_name", csv_name, + "stereo_pattern_" + currentDateTime() + ".csv"); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType + f; f = boost::bind(param_callback, _1, _2); server.setCallback(f); + ros::Subscriber warmup_sub = + nh.subscribe("warmup_switch", 1, warmup_callback); + + if (skip_warmup_) { + ROS_WARN("Skipping warmup"); + WARMUP_DONE = true; + } + + // Just for statistics + if (save_to_file_) { + ostringstream os; + os << getenv("HOME") << "/v2c_experiments/" << csv_name; + if (save_to_file_) { + if (DEBUG) ROS_INFO("Opening %s", os.str().c_str()); + savefile.open(os.str().c_str()); + savefile << "det1_x, det1_y, det1_z, det2_x, det2_y, det2_z, det3_x, " + "det3_y, det3_z, det4_x, det4_y, det4_z, cent1_x, cent1_y, " + "cent1_z, cent2_x, cent2_y, cent2_z, cent3_x, cent3_y, " + "cent3_z, cent4_x, cent4_y, cent4_z, it" + << endl; + } + } + ros::spin(); return 0; } diff --git a/src/velo2cam_calibration.cpp b/src/velo2cam_calibration.cpp index a239c7c..d7f7a0f 100644 --- a/src/velo2cam_calibration.cpp +++ b/src/velo2cam_calibration.cpp @@ -1,6 +1,7 @@ /* - velo2cam_calibration - Automatic calibration algorithm for extrinsic parameters of a stereo camera and a velodyne - Copyright (C) 2017-2018 Jorge Beltran, Carlos Guindel + velo2cam_calibration - Automatic calibration algorithm for extrinsic + parameters of a stereo camera and a velodyne Copyright (C) 2017-2021 Jorge + Beltran, Carlos Guindel This file is part of velo2cam_calibration. @@ -24,510 +25,814 @@ #define PCL_NO_PRECOMPILE -#include "velo2cam_calibration/ClusterCentroids.h" - -#include -#include -#include -#include -#include +#include #include -#include -#include - -#include -#include "tinyxml.h" - -#ifdef TF2 -#include -#include -#include -#include -#include -#include -#else +#include +#include +#include +#include +#include #include #include #include -#include -#include -#endif - - -#include "velo2cam_utils.h" +#include +#include +#include using namespace std; using namespace sensor_msgs; -ros::Publisher aux_pub, aux2_pub; -ros::Publisher t_pub; -ros::Publisher clusters_c, clusters_l; +ros::Publisher clusters_sensor2_pub, clusters_sensor1_pub; +ros::Publisher colour_sensor2_pub, colour_sensor1_pub; +ros::Publisher sensor_switch_pub; +ros::Publisher iterations_pub; int nFrames; -bool laserReceived, cameraReceived; +bool sensor1Received, sensor2Received; + +pcl::PointCloud::Ptr sensor1_cloud, sensor2_cloud; +pcl::PointCloud::Ptr isensor1_cloud, isensor2_cloud; +std::vector sensor1_vector(4), sensor2_vector(4); -pcl::PointCloud::Ptr laser_cloud, camera_cloud; -pcl::PointCloud::Ptr ilaser_cloud, icamera_cloud; -std::vector lv(4), cv(4); +tf::StampedTransform tf_sensor1_sensor2; -tf::StampedTransform tf_velodyne_camera; +bool is_sensor1_cam, is_sensor2_cam; +bool skip_warmup, single_pose_mode; +string sensor1_frame_id = ""; +string sensor1_rotated_frame_id = ""; +string sensor2_frame_id = ""; +string sensor2_rotated_frame_id = ""; typedef Eigen::Matrix Matrix12d; typedef Eigen::Matrix Vector12d; tf::Transform transf; -std::vector< std::tuple, std::vector > > laser_buffer; -std::vector< std::tuple, std::vector > > cam_buffer; +std::vector, + std::vector>>> + sensor1_buffer; +std::vector, + std::vector>>> + sensor2_buffer; + +int S1_WARMUP_COUNT = 0, S2_WARMUP_COUNT = 0; +bool S1_WARMUP_DONE = false, S2_WARMUP_DONE = false; +int TARGET_POSITIONS_COUNT = 0; +int TARGET_ITERATIONS = 30; bool sync_iterations; bool save_to_file_; bool publish_tf_; +bool calibration_ended; +bool results_every_pose; -long int laser_count, cam_count; +long int sensor1_count, sensor2_count; std::ofstream savefile; -const std::string currentDateTime() { - time_t now = time(0); - struct tm tstruct; - char buf[80]; - tstruct = *localtime(&now); - // Visit http://en.cppreference.com/w/cpp/chrono/c/strftime - // for more information about date/time format - strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct); +void sensor1_callback( + const velo2cam_calibration::ClusterCentroids::ConstPtr sensor1_centroids); +void sensor2_callback( + velo2cam_calibration::ClusterCentroids::ConstPtr sensor2_centroids); - return buf; -} +ros::NodeHandle *nh_; -void calibrateExtrinsics(int seek_iter = -1){ - // ROS_INFO("Hey man, keep calm... I'm calibrating your sensors..."); +ros::Subscriber sensor1_sub, sensor2_sub; - std::vector local_lv, local_cv; - pcl::PointCloud::Ptr local_laser_cloud, local_camera_cloud; +void calibrateExtrinsics(int seek_iter = -1) { + std::vector local_sensor1_vector, local_sensor2_vector; + pcl::PointCloud::Ptr local_sensor1_cloud( + new pcl::PointCloud), + local_sensor2_cloud(new pcl::PointCloud); pcl::PointCloud local_l_cloud, local_c_cloud; - int used_stereo, used_laser; + int used_sensor2, used_sensor1; - if (seek_iter>0){ - if(DEBUG) ROS_INFO("Seeking %d iterations", seek_iter); - if(DEBUG) ROS_INFO("Last cam: %d, last laser: %d", std::get<0>(cam_buffer.back()),std::get<0>(laser_buffer.back())); - auto it = std::find_if(cam_buffer.begin(), cam_buffer.end(), [&seek_iter](const std::tuple, std::vector >& e) {return std::get<0>(e) == seek_iter;}); - if (it == cam_buffer.end()) { - ROS_WARN("Could not sync cam"); - return; - } - - auto it2 = std::find_if(laser_buffer.begin(), laser_buffer.end(), [&seek_iter](const std::tuple, std::vector >& e) {return std::get<0>(e) == seek_iter;}); - if (it2 == laser_buffer.end()) { - ROS_WARN("Could not sync laser"); - return; - } + // Get final frame names for TF broadcaster + string sensor1_final_transformation_frame = sensor1_frame_id; + if (is_sensor1_cam) { + sensor1_final_transformation_frame = sensor1_rotated_frame_id; + } + string sensor2_final_transformation_frame = sensor2_frame_id; + if (is_sensor2_cam) { + sensor2_final_transformation_frame = sensor2_rotated_frame_id; + } - used_stereo = std::get<1>(*it); - used_laser = std::get<1>(*it2); + int total_sensor1, total_sensor2; + + if (seek_iter > 0) { // Add clouds (per sensor) from every position using + // last 'seek_iter' detection + if (DEBUG) ROS_INFO("Seeking %d iterations", seek_iter); + + for (int i = 0; i < TARGET_POSITIONS_COUNT + 1; ++i) { + if (DEBUG) + ROS_INFO("Target position: %d, Last sensor2: %d, last sensor1: %d", + i + 1, std::get<0>(sensor2_buffer[i].back()), + std::get<0>(sensor1_buffer[i].back())); + // Sensor 1 + auto it1 = std::find_if( + sensor1_buffer[i].begin(), sensor1_buffer[i].end(), + [&seek_iter]( + const std::tuple, + std::vector> &e) { + return std::get<0>(e) == seek_iter; + }); + if (it1 == sensor1_buffer[i].end()) { + ROS_WARN("Could not sync sensor1"); + return; + } - local_cv = std::get<3>(*it); - local_c_cloud = std::get<2>(*it); - local_camera_cloud = local_c_cloud.makeShared(); + local_sensor1_vector.insert( + local_sensor1_vector.end(), std::get<3>(*it1).begin(), + std::get<3>(*it1).end()); // Add sorted centers (for equations) + *local_sensor1_cloud += + std::get<2>(*it1); // Add centers cloud (for registration) + used_sensor1 = std::get<1>(*it1); + total_sensor1 = std::get<0>(*it1); + + // Sensor 2 + auto it2 = std::find_if( + sensor2_buffer[i].begin(), sensor2_buffer[i].end(), + [&seek_iter]( + const std::tuple, + std::vector> &e) { + return std::get<0>(e) == seek_iter; + }); + if (it2 == sensor2_buffer[i].end()) { + ROS_WARN("Could not sync sensor2"); + return; + } - local_lv = std::get<3>(*it2); - local_l_cloud = std::get<2>(*it2); - local_laser_cloud = local_l_cloud.makeShared(); + local_sensor2_vector.insert( + local_sensor2_vector.end(), std::get<3>(*it2).begin(), + std::get<3>(*it2).end()); // Add sorted centers (for equations) + *local_sensor2_cloud += + std::get<2>(*it2); // Add centers cloud (for registration) + used_sensor2 = std::get<1>(*it2); + total_sensor2 = std::get<0>(*it2); + } ROS_INFO("Synchronizing cluster centroids"); - }else{ - local_lv = lv; - local_cv = cv; - local_laser_cloud = laser_cloud; - local_camera_cloud = camera_cloud; + } else { // Add clouds (per sensor) from every position using last available + // detection + for (int i = 0; i < TARGET_POSITIONS_COUNT + 1; ++i) { + // Sensor 1 + local_sensor1_vector.insert( + local_sensor1_vector.end(), + std::get<3>(sensor1_buffer[i].back()).begin(), + std::get<3>(sensor1_buffer[i].back()) + .end()); // Add sorted centers (for equations) + *local_sensor1_cloud += std::get<2>( + sensor1_buffer[i].back()); // Add centers cloud (for registration) + used_sensor1 = std::get<1>(sensor2_buffer[i].back()); + + // Sensor 2 + local_sensor2_vector.insert( + local_sensor2_vector.end(), + std::get<3>(sensor2_buffer[i].back()).begin(), + std::get<3>(sensor2_buffer[i].back()) + .end()); // Add sorted centers (for equations) + *local_sensor2_cloud += std::get<2>( + sensor2_buffer[i].back()); // Add centers cloud (for registration) + } } - sensor_msgs::PointCloud2 ros_cloud; - pcl::toROSMsg(*local_camera_cloud, ros_cloud); - ros_cloud.header.frame_id = "stereo"; - clusters_c.publish(ros_cloud); - - pcl::toROSMsg(*local_laser_cloud, ros_cloud); - ros_cloud.header.frame_id = "velodyne"; - clusters_l.publish(ros_cloud); - - Vector12d las_12; - las_12 << local_lv[0].x, - local_lv[0].y, - local_lv[0].z, - local_lv[1].x, - local_lv[1].y, - local_lv[1].z, - local_lv[2].x, - local_lv[2].y, - local_lv[2].z, - local_lv[3].x, - local_lv[3].y, - local_lv[3].z; - - Vector12d cam_12; - cam_12 << local_cv[0].x, - local_cv[0].y, - local_cv[0].z, - local_cv[1].x, - local_cv[1].y, - local_cv[1].z, - local_cv[2].x, - local_cv[2].y, - local_cv[2].z, - local_cv[3].x, - local_cv[3].y, - local_cv[3].z; - - Vector12d diff_12; - diff_12 = las_12-cam_12; - - Eigen::MatrixXd matrix_transl(12,3); - matrix_transl << 1, 0, 0, 0, 1, 0, 0, 0, 1, - 1, 0, 0, 0, 1, 0, 0, 0, 1, - 1, 0, 0, 0, 1, 0, 0, 0, 1, - 1, 0, 0, 0, 1, 0, 0, 0, 1; - - Eigen::Vector3d x; - x = matrix_transl.colPivHouseholderQr().solve(diff_12); - - // cout << "The least-squares solution is:\n" - // << x << endl; - - Eigen::Matrix4f Tm; - Tm << 1, 0, 0, x[0], - 0, 1, 0, x[1], - 0, 0, 1, x[2], - 0, 0, 0, 1; - - if(DEBUG) ROS_INFO("Step 1: Translation"); - if(DEBUG) cout << Tm << endl; - - pcl::PointCloud::Ptr translated_pc (new pcl::PointCloud ()); - pcl::transformPointCloud(*local_camera_cloud, *translated_pc, Tm); - - pcl::toROSMsg(*translated_pc, ros_cloud); - ros_cloud.header.frame_id = "velodyne"; - t_pub.publish(ros_cloud); - - pcl::IterativeClosestPoint icp; - icp.setInputSource(translated_pc); - icp.setInputTarget(local_laser_cloud); - pcl::PointCloud Final; - icp.align(Final); - icp.setMaxCorrespondenceDistance(0.2); - icp.setMaximumIterations(1000); - if (icp.hasConverged()){ - if(DEBUG) ROS_INFO("ICP Converged. Score: %lf", icp.getFitnessScore()); - }else{ - ROS_WARN("ICP failed to converge"); - return; + if (DEBUG) { + sensor_msgs::PointCloud2 ros_cloud; + pcl::toROSMsg(*local_sensor2_cloud, ros_cloud); + ros_cloud.header.frame_id = sensor2_rotated_frame_id; + clusters_sensor2_pub.publish(ros_cloud); + + pcl::toROSMsg(*local_sensor1_cloud, ros_cloud); + ros_cloud.header.frame_id = sensor1_frame_id; + clusters_sensor1_pub.publish(ros_cloud); } - if(DEBUG) ROS_INFO("Step 2. ICP Transformation:"); - if(DEBUG) cout << icp.getFinalTransformation() << std::endl; - Eigen::Matrix4f transformation = icp.getFinalTransformation (); - Eigen::Matrix4f final_trans = transformation * Tm; + // SVD code + pcl::PointCloud::Ptr sorted_centers1( + new pcl::PointCloud()); + pcl::PointCloud::Ptr sorted_centers2( + new pcl::PointCloud()); - tf::Matrix3x3 tf3d; - tf3d.setValue(final_trans(0,0), final_trans(0,1), final_trans(0,2), - final_trans(1,0), final_trans(1,1), final_trans(1,2), - final_trans(2,0), final_trans(2,1), final_trans(2,2)); + for (int i = 0; i < local_sensor1_vector.size(); ++i) { + sorted_centers1->push_back(local_sensor1_vector[i]); + sorted_centers2->push_back(local_sensor2_vector[i]); + } + + Eigen::Matrix4f final_transformation; + const pcl::registration::TransformationEstimationSVD + trans_est_svd(true); + trans_est_svd.estimateRigidTransformation(*sorted_centers1, *sorted_centers2, + final_transformation); - if(DEBUG) ROS_INFO("Final Transformation"); - if(DEBUG) cout << final_trans << endl; + tf::Matrix3x3 tf3d; + tf3d.setValue(final_transformation(0, 0), final_transformation(0, 1), + final_transformation(0, 2), final_transformation(1, 0), + final_transformation(1, 1), final_transformation(1, 2), + final_transformation(2, 0), final_transformation(2, 1), + final_transformation(2, 2)); tf::Quaternion tfqt; tf3d.getRotation(tfqt); - #ifdef TF2 - - static tf2_ros::TransformBroadcaster br; - geometry_msgs::TransformStamped transformStamped; - - transformStamped.header.stamp = ros::Time::now(); - transformStamped.header.frame_id = "velodyne"; - transformStamped.child_frame_id = "stereo"; - transformStamped.transform.translation.x = final_trans(0,3); - transformStamped.transform.translation.y = final_trans(1,3); - transformStamped.transform.translation.z = final_trans(2,3); - transformStamped.transform.rotation.x = tfqt.x(); - transformStamped.transform.rotation.y = tfqt.y(); - transformStamped.transform.rotation.z = tfqt.z(); - transformStamped.transform.rotation.w = tfqt.w(); - - br.sendTransform(transformStamped); - - #else - tf::Vector3 origin; - origin.setValue(final_trans(0,3),final_trans(1,3),final_trans(2,3)); + origin.setValue(final_transformation(0, 3), final_transformation(1, 3), + final_transformation(2, 3)); transf.setOrigin(origin); transf.setRotation(tfqt); - #endif - static tf::TransformBroadcaster br; - tf_velodyne_camera = tf::StampedTransform(transf, ros::Time::now(), "velodyne", "stereo"); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); + tf_sensor1_sensor2 = tf::StampedTransform(transf.inverse(), ros::Time::now(), + sensor1_final_transformation_frame, + sensor2_final_transformation_frame); + if (publish_tf_) br.sendTransform(tf_sensor1_sensor2); - tf::Transform inverse = tf_velodyne_camera.inverse(); + tf::Transform inverse = tf_sensor1_sensor2.inverse(); double roll, pitch, yaw; - double xt = inverse.getOrigin().getX(), yt = inverse.getOrigin().getY(), zt = inverse.getOrigin().getZ(); + double xt = inverse.getOrigin().getX(), yt = inverse.getOrigin().getY(), + zt = inverse.getOrigin().getZ(); inverse.getBasis().getRPY(roll, pitch, yaw); - if (save_to_file_){ - savefile << seek_iter << ", " << xt << ", " << yt << ", " << zt << ", " << roll << ", " << pitch << ", " << yaw << ", " << used_laser << ", " << used_stereo << endl; + if (save_to_file_) { + savefile << seek_iter << ", " << xt << ", " << yt << ", " << zt << ", " + << roll << ", " << pitch << ", " << yaw << ", " << used_sensor1 + << ", " << used_sensor2 << ", " << total_sensor1 << ", " + << total_sensor2 << endl; } - ROS_INFO("[V2C] Calibration result:"); - ROS_INFO("x=%.4f y=%.4f z=%.4f",xt,yt,zt); - ROS_INFO("roll=%.4f, pitch=%.4f, yaw=%.4f",roll,pitch,yaw); - // ROS_INFO("Translation matrix"); - // ROS_INFO("%.4f, %.4f, %.4f",inverse.getBasis()[0][0],inverse.getBasis()[0][1],inverse.getBasis()[0][2]); - // ROS_INFO("%.4f, %.4f, %.4f",inverse.getBasis()[1][0],inverse.getBasis()[1][1],inverse.getBasis()[1][2]); - // ROS_INFO("%.4f, %.4f, %.4f",inverse.getBasis()[2][0],inverse.getBasis()[2][1],inverse.getBasis()[2][2]); + cout << setprecision(4) << std::fixed; + cout << "Calibration finished succesfully." << endl; + cout << "Extrinsic parameters:" << endl; + cout << "x = " << xt << "\ty = " << yt << "\tz = " << zt << endl; + cout << "roll = " << roll << "\tpitch = " << pitch << "\tyaw = " << yaw << endl; - laserReceived = false; - cameraReceived = false; + sensor1Received = false; + sensor2Received = false; } -void laser_callback(const velo2cam_calibration::ClusterCentroids::ConstPtr velo_centroids){ - // ROS_INFO("Velodyne pattern ready!"); - laserReceived = true; - - fromROSMsg(velo_centroids->cloud, *laser_cloud); +void sensor1_callback( + const velo2cam_calibration::ClusterCentroids::ConstPtr sensor1_centroids) { + sensor1_frame_id = sensor1_centroids->header.frame_id; + if (!S1_WARMUP_DONE) { + S1_WARMUP_COUNT++; + cout << "Clusters from " << sensor1_frame_id << ": " << S1_WARMUP_COUNT + << "/10" << '\r' << flush; + if (S1_WARMUP_COUNT >= 10) // TODO: Change to param? + { + cout << endl; + sensor1_sub.shutdown(); + sensor2_sub.shutdown(); + + cout << "Clusters from " << sensor1_frame_id + << " received. Is the warmup done? [Y/n]" << endl; + string answer; + getline(cin, answer); + if (answer == "y" || answer == "Y" || answer == "") { + S1_WARMUP_DONE = !S1_WARMUP_DONE; + + if (!S2_WARMUP_DONE) { + cout << "Filters for sensor 1 are adjusted now. Please, proceed with " + "the other sensor." + << endl; + } else { // Both sensors adjusted + cout << "Warmup phase completed. Starting calibration phase." << endl; + std_msgs::Empty myMsg; + sensor_switch_pub.publish(myMsg); // + } + } else { // Reset counter to allow further warmup + S1_WARMUP_COUNT = 0; + } - sortPatternCentersYZ(laser_cloud, lv); - colourCenters(laser_cloud, ilaser_cloud); + sensor1_sub = nh_->subscribe( + "cloud1", 100, sensor1_callback); + sensor2_sub = nh_->subscribe( + "cloud2", 100, sensor2_callback); + } + return; + } - laser_buffer.push_back(std::tuple, std::vector >(velo_centroids->total_iterations, velo_centroids->cluster_iterations, *laser_cloud,lv)); - laser_count = velo_centroids->total_iterations; + if (!S2_WARMUP_DONE) { + return; + } - if(DEBUG) ROS_INFO("[V2C] LASER"); + if (DEBUG) ROS_INFO("sensor1 (%s) pattern ready!", sensor1_frame_id.c_str()); - for(vector::iterator it=lv.begin(); it= laser_count){ - calibrateExtrinsics(laser_count); + if (is_sensor1_cam) { + std::ostringstream sstream; + sstream << "rotated_" << sensor1_frame_id; + sensor1_rotated_frame_id = sstream.str(); + + pcl::PointCloud::Ptr xy_sensor1_cloud( + new pcl::PointCloud()); + + fromROSMsg(sensor1_centroids->cloud, *xy_sensor1_cloud); + + tf::TransformListener listener; + tf::StampedTransform transform; + try { + listener.waitForTransform(sensor1_rotated_frame_id, sensor1_frame_id, + ros::Time(0), ros::Duration(20.0)); + listener.lookupTransform(sensor1_rotated_frame_id, sensor1_frame_id, + ros::Time(0), transform); + } catch (tf::TransformException &ex) { + ROS_WARN("TF exception:\n%s", ex.what()); return; - }else{ - if (tf_velodyne_camera.frame_id_ != ""){ - static tf::TransformBroadcaster br; - tf_velodyne_camera.stamp_ = ros::Time::now(); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); - return; - } } - } - if(laserReceived && cameraReceived){ - calibrateExtrinsics(); - }else{ - static tf::TransformBroadcaster br; - tf_velodyne_camera.stamp_ = ros::Time::now(); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); - } -} + tf::Transform inverse = transform.inverse(); + double roll, pitch, yaw; + inverse.getBasis().getRPY(roll, pitch, yaw); -void stereo_callback(velo2cam_calibration::ClusterCentroids::ConstPtr image_centroids){ - // if(DEBUG) ROS_INFO("Camera pattern ready!"); + pcl_ros::transformPointCloud(*xy_sensor1_cloud, *sensor1_cloud, transform); + } else { + fromROSMsg(sensor1_centroids->cloud, *sensor1_cloud); + } -#ifdef TF2 + sensor1Received = true; - //TODO: adapt to ClusterCentroids + sortPatternCenters(sensor1_cloud, sensor1_vector); + if (DEBUG) { + colourCenters(sensor1_vector, isensor1_cloud); - PointCloud2 xy_image_cloud; + sensor_msgs::PointCloud2 colour_cloud; + pcl::toROSMsg(*isensor1_cloud, colour_cloud); + colour_cloud.header.frame_id = + is_sensor1_cam ? sensor1_rotated_frame_id : sensor1_frame_id; + colour_sensor1_pub.publish(colour_cloud); + } - tf2_ros::Buffer tfBuffer; - tf2_ros::TransformListener tfListener(tfBuffer); - geometry_msgs::TransformStamped transformStamped; - try{ - transformStamped = tfBuffer.lookupTransform("stereo", "stereo_camera", - ros::Time(0), ros::Duration(20)); + sensor1_buffer[TARGET_POSITIONS_COUNT].push_back( + std::tuple, + std::vector>( + sensor1_centroids->total_iterations, + sensor1_centroids->cluster_iterations, *sensor1_cloud, + sensor1_vector)); + sensor1_count = sensor1_centroids->total_iterations; + + if (DEBUG) ROS_INFO("[V2C] sensor1"); + + for (vector::iterator it = sensor1_vector.begin(); + it < sensor1_vector.end(); ++it) { + if (DEBUG) + cout << "l" << it - sensor1_vector.begin() << "=" + << "[" << (*it).x << " " << (*it).y << " " << (*it).z << "]" << endl; } - catch (tf2::TransformException &ex) { - ROS_WARN("%s",ex.what()); - ros::Duration(1.0).sleep(); + + // sync_iterations is designed to extract a calibration result every single + // frame, so we cannot wait until TARGET_ITERATIONS + if (sync_iterations) { + if (sensor2_count >= sensor1_count) { + calibrateExtrinsics(sensor1_count); + } else { + if (tf_sensor1_sensor2.frame_id_ != "" && + tf_sensor1_sensor2.child_frame_id_ != "") { + static tf::TransformBroadcaster br; + tf_sensor1_sensor2.stamp_ = ros::Time::now(); + if (publish_tf_) br.sendTransform(tf_sensor1_sensor2); + } + } return; } - tf2::doTransform (*image_cloud, xy_image_cloud, transformStamped); - fromROSMsg(xy_image_cloud, *camera_cloud); - -#else - - pcl::PointCloud::Ptr xy_camera_cloud (new pcl::PointCloud ()); - fromROSMsg(image_centroids->cloud, *xy_camera_cloud); + // Normal operation (sync_iterations=false) + if (sensor1Received && sensor2Received) { + cout << min(sensor1_count, sensor2_count) << "/30 iterations" << '\r' << flush; + + std_msgs::Int32 it; + it.data = min(sensor1_count, sensor2_count); + iterations_pub.publish(it); + + if (sensor1_count >= TARGET_ITERATIONS && + sensor2_count >= TARGET_ITERATIONS) { + cout << endl; + sensor1_sub.shutdown(); + sensor2_sub.shutdown(); + + string answer; + if (single_pose_mode) { + answer = "n"; + } else { + cout << "Target iterations reached. Do you need another target " + "location? [y/N]" + << endl; + getline(cin, answer); + } + if (answer == "n" || answer == "N" || answer == "") { + calibrateExtrinsics(-1); + calibration_ended = true; + } else { // Move the target and start over + if (results_every_pose) calibrateExtrinsics(-1); + TARGET_POSITIONS_COUNT++; + cout << "Please, move the target to its new position and adjust the " + "filters for each sensor before the calibration starts." + << endl; + // Start over if other position of the target is required + std_msgs::Empty myMsg; + sensor_switch_pub.publish(myMsg); // Set sensor nodes to warmup phase + S1_WARMUP_DONE = false; + S1_WARMUP_COUNT = 0; + S2_WARMUP_DONE = false; + S2_WARMUP_COUNT = 0; + sensor1Received = false; + sensor2Received = false; + sensor1_count = 0; + sensor2_count = 0; + } + sensor1_sub = nh_->subscribe( + "cloud1", 100, sensor1_callback); + sensor2_sub = nh_->subscribe( + "cloud2", 100, sensor2_callback); + return; + } + } else { + if (tf_sensor1_sensor2.frame_id_ != "" && + tf_sensor1_sensor2.child_frame_id_ != "") { + static tf::TransformBroadcaster br; + tf_sensor1_sensor2.stamp_ = ros::Time::now(); + if (publish_tf_) br.sendTransform(tf_sensor1_sensor2); + } + } +} - tf::TransformListener listener; - tf::StampedTransform transform; - try{ - listener.waitForTransform("stereo", "stereo_camera", ros::Time(0), ros::Duration(20.0)); - listener.lookupTransform ("stereo", "stereo_camera", ros::Time(0), transform); - }catch (tf::TransformException& ex) { - ROS_WARN("TF exception:\n%s", ex.what()); +void sensor2_callback( + velo2cam_calibration::ClusterCentroids::ConstPtr sensor2_centroids) { + sensor2_frame_id = sensor2_centroids->header.frame_id; + if (!S2_WARMUP_DONE && S1_WARMUP_DONE) { + S2_WARMUP_COUNT++; + cout << "Clusters from " << sensor2_frame_id << ": " << S2_WARMUP_COUNT + << "/10" << '\r' << flush; + if (S2_WARMUP_COUNT >= 10) // TODO: Change to param? + { + cout << endl; + sensor1_sub.shutdown(); + sensor2_sub.shutdown(); + + cout << "Clusters from " << sensor2_frame_id + << " received. Is the warmup done? (you can also reset this " + "position) [Y/n/r]" + << endl; + string answer; + getline(cin, answer); + if (answer == "y" || answer == "Y" || answer == "") { + S2_WARMUP_DONE = !S2_WARMUP_DONE; + + if (!S1_WARMUP_DONE) { + cout << "Filters for sensor 2 are adjusted now. Please, proceed with " + "the other sensor." + << endl; + } else { // Both sensors adjusted + cout << "Warmup phase completed. Starting calibration phase." << endl; + std_msgs::Empty myMsg; + sensor_switch_pub.publish(myMsg); // + } + } else if (answer == "r" || + answer == "R") { // Reset this position and + // go back to Sensor 1 warmup + S1_WARMUP_DONE = false; + S1_WARMUP_COUNT = 0; + S2_WARMUP_DONE = false; + S2_WARMUP_COUNT = 0; + sensor1Received = false; + sensor2Received = false; + sensor1_count = 0; + sensor2_count = 0; + cout << "Please, adjust the filters for each sensor before the " + "calibration starts." + << endl; + } else { // Reset counter to allow further warmup + S2_WARMUP_COUNT = 0; + } + sensor1_sub = nh_->subscribe( + "cloud1", 100, sensor1_callback); + sensor2_sub = nh_->subscribe( + "cloud2", 100, sensor2_callback); + } + return; + } else if (!S2_WARMUP_DONE) { return; } + if (DEBUG) ROS_INFO("sensor2 (%s) pattern ready!", sensor2_frame_id.c_str()); - tf::Transform inverse = transform.inverse(); - double roll, pitch, yaw; - inverse.getBasis().getRPY(roll, pitch, yaw); + if (sensor2_buffer.size() == TARGET_POSITIONS_COUNT) { + sensor2_buffer.resize(TARGET_POSITIONS_COUNT + 1); + } + + if (is_sensor2_cam) { + std::ostringstream sstream; + sstream << "rotated_" << sensor2_frame_id; + sensor2_rotated_frame_id = sstream.str(); + + pcl::PointCloud::Ptr xy_sensor2_cloud( + new pcl::PointCloud()); + + fromROSMsg(sensor2_centroids->cloud, *xy_sensor2_cloud); + + tf::TransformListener listener; + tf::StampedTransform transform; + try { + listener.waitForTransform(sensor2_rotated_frame_id, sensor2_frame_id, + ros::Time(0), ros::Duration(20.0)); + listener.lookupTransform(sensor2_rotated_frame_id, sensor2_frame_id, + ros::Time(0), transform); + } catch (tf::TransformException &ex) { + ROS_WARN("TF exception:\n%s", ex.what()); + return; + } - pcl_ros::transformPointCloud (*xy_camera_cloud, *camera_cloud, transform); + tf::Transform inverse = transform.inverse(); + double roll, pitch, yaw; + inverse.getBasis().getRPY(roll, pitch, yaw); -#endif + pcl_ros::transformPointCloud(*xy_sensor2_cloud, *sensor2_cloud, transform); + } else { + fromROSMsg(sensor2_centroids->cloud, *sensor2_cloud); + } - cameraReceived = true; + sensor2Received = true; - sortPatternCentersYZ(camera_cloud, cv); - colourCenters(camera_cloud, icamera_cloud); + sortPatternCenters(sensor2_cloud, sensor2_vector); - cam_buffer.push_back(std::tuple, std::vector >(image_centroids->total_iterations,image_centroids->cluster_iterations,*camera_cloud,cv)); - cam_count = image_centroids->total_iterations; + if (DEBUG) { + colourCenters(sensor2_vector, isensor2_cloud); - if(DEBUG) ROS_INFO("[V2C] CAMERA"); + sensor_msgs::PointCloud2 colour_cloud; + pcl::toROSMsg(*isensor2_cloud, colour_cloud); + colour_cloud.header.frame_id = + is_sensor2_cam ? sensor2_rotated_frame_id : sensor2_frame_id; + colour_sensor2_pub.publish(colour_cloud); + } - for(vector::iterator it=cv.begin(); it, + std::vector>( + sensor2_centroids->total_iterations, + sensor2_centroids->cluster_iterations, *sensor2_cloud, + sensor2_vector)); + sensor2_count = sensor2_centroids->total_iterations; + + if (DEBUG) ROS_INFO("[V2C] sensor2"); + + for (vector::iterator it = sensor2_vector.begin(); + it < sensor2_vector.end(); ++it) { + if (DEBUG) + cout << "c" << it - sensor2_vector.begin() << "=" + << "[" << (*it).x << " " << (*it).y << " " << (*it).z << "]" << endl; } - if (sync_iterations){ - if(laser_count >= cam_count){ - calibrateExtrinsics(cam_count); - return; - }else{ - if (tf_velodyne_camera.frame_id_ != ""){ + // sync_iterations is designed to extract a calibration result every single + // frame, so we cannot wait until TARGET_ITERATIONS + if (sync_iterations) { + if (sensor1_count >= sensor2_count) { + calibrateExtrinsics(sensor2_count); + } else { + if (tf_sensor1_sensor2.frame_id_ != "" && + tf_sensor1_sensor2.child_frame_id_ != "") { static tf::TransformBroadcaster br; - tf_velodyne_camera.stamp_ = ros::Time::now(); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); - return; + tf_sensor1_sensor2.stamp_ = ros::Time::now(); + if (publish_tf_) br.sendTransform(tf_sensor1_sensor2); } } + return; } - if(laserReceived && cameraReceived){ - if(DEBUG) ROS_INFO("[V2C] Calibrating..."); - calibrateExtrinsics(); - }else{ - static tf::TransformBroadcaster br; - tf_velodyne_camera.stamp_ = ros::Time::now(); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); + // Normal operation (sync_iterations=false) + if (sensor1Received && sensor2Received) { + cout << min(sensor1_count, sensor2_count) << "/30 iterations" << '\r' << flush; + + std_msgs::Int32 it; + it.data = min(sensor1_count, sensor2_count); + iterations_pub.publish(it); + + if (sensor1_count >= TARGET_ITERATIONS && + sensor2_count >= TARGET_ITERATIONS) { + cout << endl; + sensor1_sub.shutdown(); + sensor2_sub.shutdown(); + + string answer; + if (single_pose_mode) { + answer = "n"; + } else { + cout << "Target iterations reached. Do you need another target " + "location? [y/N]" + << endl; + getline(cin, answer); + } + if (answer == "n" || answer == "N" || answer == "") { + calibrateExtrinsics(-1); + calibration_ended = true; + } else { // Move the target and start over + if (results_every_pose) calibrateExtrinsics(-1); + TARGET_POSITIONS_COUNT++; + cout << "Please, move the target to its new position and adjust the " + "filters for each sensor before the calibration starts." + << endl; + // Start over if other position of the target is required + std_msgs::Empty myMsg; + sensor_switch_pub.publish(myMsg); // Set sensor nodes to warmup phase + S1_WARMUP_DONE = false; + S1_WARMUP_COUNT = 0; + S2_WARMUP_DONE = false; + S2_WARMUP_COUNT = 0; + sensor1Received = false; + sensor2Received = false; + sensor1_count = 0; + sensor2_count = 0; + } + sensor1_sub = nh_->subscribe( + "cloud1", 100, sensor1_callback); + sensor2_sub = nh_->subscribe( + "cloud2", 100, sensor2_callback); + return; + } + } else { + if (tf_sensor1_sensor2.frame_id_ != "" && + tf_sensor1_sensor2.child_frame_id_ != "") { + static tf::TransformBroadcaster br; + tf_sensor1_sensor2.stamp_ = ros::Time::now(); + if (publish_tf_) br.sendTransform(tf_sensor1_sensor2); + } } } -int main(int argc, char **argv){ +int main(int argc, char **argv) { ros::init(argc, argv, "velo2cam_calibration"); - ros::NodeHandle nh_("~"); // LOCAL - - nh_.param("sync_iterations", sync_iterations, false); - nh_.param("save_to_file", save_to_file_, false); - nh_.param("publish_tf", publish_tf_, true); - - static tf::TransformBroadcaster br; - tf_velodyne_camera = tf::StampedTransform(tf::Transform::getIdentity(), ros::Time::now(), "velodyne", "stereo"); - if (publish_tf_) br.sendTransform(tf_velodyne_camera); - - laserReceived = false; - laser_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - ilaser_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - cameraReceived = false; - camera_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - icamera_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); + ros::NodeHandle nh; // GLOBAL + nh_ = new ros::NodeHandle("~"); // LOCAL + + string csv_name; + + nh_->param("sync_iterations", sync_iterations, false); + nh_->param("save_to_file", save_to_file_, false); + nh_->param("publish_tf", publish_tf_, true); + nh_->param("is_sensor2_cam", is_sensor2_cam, false); + nh_->param("is_sensor1_cam", is_sensor1_cam, false); + nh_->param("skip_warmup", skip_warmup, false); + nh_->param("single_pose_mode", single_pose_mode, false); + nh_->param("results_every_pose", results_every_pose, false); + nh_->param("csv_name", csv_name, + "registration_" + currentDateTime() + ".csv"); + + sensor1Received = false; + sensor1_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + isensor1_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + sensor2Received = false; + sensor2_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + isensor2_cloud = + pcl::PointCloud::Ptr(new pcl::PointCloud); + + sensor1_sub = nh_->subscribe( + "cloud1", 100, sensor1_callback); + sensor2_sub = nh_->subscribe( + "cloud2", 100, sensor2_callback); + + if (DEBUG) { + clusters_sensor2_pub = + nh_->advertise("clusters_sensor2", 1); + clusters_sensor1_pub = + nh_->advertise("clusters_sensor1", 1); + + colour_sensor2_pub = + nh_->advertise("colour_sensor2", 1); + colour_sensor1_pub = + nh_->advertise("colour_sensor1", 1); + } - ros::Subscriber laser_sub = nh_.subscribe("cloud1", 1, laser_callback); - ros::Subscriber stereo_sub = nh_.subscribe("cloud2", 1, stereo_callback); + sensor_switch_pub = nh.advertise("warmup_switch", 1); + iterations_pub = nh_->advertise("iterations", 1); - t_pub = nh_.advertise("translated_cloud", 1); - clusters_c = nh_.advertise("clusters_camera", 1); - clusters_l = nh_.advertise("clusters_laser", 1); + calibration_ended = false; - if (save_to_file_){ + if (save_to_file_) { ostringstream os; - os << getenv("HOME") << "/results_" << currentDateTime() << ".csv" ; - if (save_to_file_){ - if(DEBUG) ROS_INFO("Opening %s", os.str().c_str()); - savefile.open (os.str().c_str()); - savefile << "it, x, y, z, r, p, y, used_l, used_c" << endl; + os << getenv("HOME") << "/v2c_experiments/" << csv_name; + if (save_to_file_) { + if (DEBUG) ROS_INFO("Opening %s", os.str().c_str()); + savefile.open(os.str().c_str()); + savefile << "it, x, y, z, r, p, y, used_sen1, used_sen2, total_sen1, " + "total_sen2" + << endl; } } + if (skip_warmup) { + S1_WARMUP_DONE = true; + S2_WARMUP_DONE = true; + ROS_WARN("Skipping warmup"); + } else { + cout << "Please, adjust the filters for each sensor before the calibration " + "starts." + << endl; + } + ros::Rate loop_rate(30); - while(ros::ok()){ + while (ros::ok() && !calibration_ended) { ros::spinOnce(); } + sensor1_sub.shutdown(); + sensor2_sub.shutdown(); + if (save_to_file_) savefile.close(); // Save calibration params to launch file for testing // Get time time_t rawtime; - struct tm * timeinfo; + struct tm *timeinfo; char buffer[80]; - time (&rawtime); + time(&rawtime); timeinfo = localtime(&rawtime); - strftime(buffer,80,"%Y-%m-%d-%H-%M-%S", timeinfo); + strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", timeinfo); std::string str(buffer); // Get tf data - tf::Transform inverse = tf_velodyne_camera.inverse(); + tf::Transform inverse = tf_sensor1_sensor2.inverse(); double roll, pitch, yaw; - double xt = inverse.getOrigin().getX(), yt = inverse.getOrigin().getY(), zt = inverse.getOrigin().getZ(); + double xt = inverse.getOrigin().getX(), yt = inverse.getOrigin().getY(), + zt = inverse.getOrigin().getZ(); inverse.getBasis().getRPY(roll, pitch, yaw); - ROS_INFO("Calibration finished succesfully..."); - ROS_INFO("x=%.4f y=%.4f z=%.4f",xt,yt,zt); - ROS_INFO("roll=%.4f, pitch=%.4f, yaw=%.4f", roll, pitch, yaw); - std::string path = ros::package::getPath("velo2cam_calibration"); - string backuppath = path + "/launch/calibrated_tf_"+ str +".launch"; + string backuppath = path + "/launch/calibrated_tf_" + str + ".launch"; path = path + "/launch/calibrated_tf.launch"; - cout << endl << "Creating .launch file with calibrated TF in: "<< endl << path.c_str() << endl; + cout << endl + << "Creating .launch file with calibrated TF in: " << endl + << path.c_str() << endl; // Create .launch file with calibrated TF TiXmlDocument doc; - TiXmlDeclaration * decl = new TiXmlDeclaration( "1.0", "utf-8", ""); - doc.LinkEndChild( decl ); - TiXmlElement * root = new TiXmlElement( "launch" ); - doc.LinkEndChild( root ); - - TiXmlElement * arg = new TiXmlElement( "arg" ); - arg->SetAttribute("name","stdout"); - arg->SetAttribute("default","screen"); - root->LinkEndChild( arg ); - - string stereo_rotation = "0 0 0 -1.57079632679 0 -1.57079632679 stereo stereo_camera 10"; + TiXmlDeclaration *decl = new TiXmlDeclaration("1.0", "utf-8", ""); + doc.LinkEndChild(decl); + TiXmlElement *root = new TiXmlElement("launch"); + doc.LinkEndChild(root); + + TiXmlElement *arg = new TiXmlElement("arg"); + arg->SetAttribute("name", "stdout"); + arg->SetAttribute("default", "screen"); + root->LinkEndChild(arg); + + string sensor2_final_transformation_frame = sensor2_frame_id; + if (is_sensor2_cam) { + sensor2_final_transformation_frame = sensor2_rotated_frame_id; + std::ostringstream sensor2_rot_stream_pub; + sensor2_rot_stream_pub << "0 0 0 -1.57079632679 0 -1.57079632679 " + << sensor2_rotated_frame_id << " " + << sensor2_frame_id << " 10"; + string sensor2_rotation = sensor2_rot_stream_pub.str(); + + TiXmlElement *sensor2_rotation_node = new TiXmlElement("node"); + sensor2_rotation_node->SetAttribute("pkg", "tf"); + sensor2_rotation_node->SetAttribute("type", "static_transform_publisher"); + sensor2_rotation_node->SetAttribute("name", "sensor2_rot_tf"); + sensor2_rotation_node->SetAttribute("args", sensor2_rotation); + root->LinkEndChild(sensor2_rotation_node); + } - TiXmlElement * stereo_rotation_node = new TiXmlElement( "node" ); - stereo_rotation_node->SetAttribute("pkg","tf"); - stereo_rotation_node->SetAttribute("type","static_transform_publisher"); - stereo_rotation_node->SetAttribute("name","stereo_ros_tf"); - stereo_rotation_node->SetAttribute("args", stereo_rotation); - root->LinkEndChild( stereo_rotation_node ); + string sensor1_final_transformation_frame = sensor1_frame_id; + if (is_sensor1_cam) { + sensor1_final_transformation_frame = sensor1_rotated_frame_id; + std::ostringstream sensor1_rot_stream_pub; + sensor1_rot_stream_pub << "0 0 0 -1.57079632679 0 -1.57079632679 " + << sensor1_rotated_frame_id << " " + << sensor1_frame_id << " 10"; + string sensor1_rotation = sensor1_rot_stream_pub.str(); + + TiXmlElement *sensor1_rotation_node = new TiXmlElement("node"); + sensor1_rotation_node->SetAttribute("pkg", "tf"); + sensor1_rotation_node->SetAttribute("type", "static_transform_publisher"); + sensor1_rotation_node->SetAttribute("name", "sensor1_rot_tf"); + sensor1_rotation_node->SetAttribute("args", sensor1_rotation); + root->LinkEndChild(sensor1_rotation_node); + } std::ostringstream sstream; - sstream << xt << " " << yt << " " << zt << " " << yaw << " " <SetAttribute("pkg","tf"); - node->SetAttribute("type","static_transform_publisher"); - node->SetAttribute("name","l2c_tf"); + TiXmlElement *node = new TiXmlElement("node"); + node->SetAttribute("pkg", "tf"); + node->SetAttribute("type", "static_transform_publisher"); + node->SetAttribute("name", "velo2cam_tf"); node->SetAttribute("args", tf_args); - root->LinkEndChild( node ); + root->LinkEndChild(node); // Save XML file and copy doc.SaveFile(path); doc.SaveFile(backuppath); - if(DEBUG) cout << "Calibration process finished." << endl; + if (DEBUG) cout << "Calibration process finished." << endl; return 0; }