├── .gitignore
├── dependencies
    └── maplab-ros-common
    │   ├── CMakeLists.txt
    │   ├── include
    │       └── maplab-ros-common
    │       │   ├── gflags-interface.h
    │       │   ├── image-conversion.h
    │       │   ├── rosbag-tools.h
    │       │   └── implementation
    │       │       └── rosbag-tools.h
    │   ├── package.xml
    │   └── src
    │       └── gflags-interface.cc
├── hough2map
    ├── share
    │   └── calibration.yaml
    ├── src
    │   ├── node.cpp
    │   └── detector.cpp
    ├── CMakeLists.txt
    ├── package.xml
    ├── launch
    │   └── hough2map.launch
    └── include
    │   └── hough2map
    │       └── detector.h
├── .gitmodules
├── README.md
└── LICENSE


/.gitignore:
--------------------------------------------------------------------------------
1 | # VS Code Files
2 | .vscode
3 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(maplab_ros_common)
 3 | 
 4 | find_package(catkin_simple REQUIRED)
 5 | catkin_simple(ALL_DEPS_REQUIRED)
 6 | 
 7 | include_directories(${catkin_INCLUDE_DIRS})
 8 | 
 9 | cs_add_library(${PROJECT_NAME}  src/gflags-interface.cc)
10 | 
11 | cs_install()
12 | cs_export()
13 | 


--------------------------------------------------------------------------------
/hough2map/share/calibration.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | cam0:
 3 |   cam_overlaps: []
 4 |   camera_model: pinhole
 5 |   distortion_coeffs: [-0.39068253024946936, 0.16263895515155055, -1.549676144742955e-05,
 6 |     -1.546082374713836e-05]
 7 |   distortion_model: radtan
 8 |   intrinsics: [222.83392335419495, 222.66595496588755, 116.2311972651792, 83.0270945314079]
 9 |   resolution: [240, 180]
10 |   rostopic: /dvs/image_raw
11 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/include/maplab-ros-common/gflags-interface.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPLAB_ROS_COMMON_GFLAGS_INTERFACE_H_
 2 | #define MAPLAB_ROS_COMMON_GFLAGS_INTERFACE_H_
 3 | 
 4 | #include <gflags/gflags.h>
 5 | #include <glog/logging.h>
 6 | #include <ros/ros.h>
 7 | 
 8 | namespace ros_common {
 9 | 
10 | void parseGflagsFromRosParams(
11 |     const char* program_name, const ros::NodeHandle& nh_private);
12 | 
13 | }  // namespace ros_common
14 | 
15 | #endif  // MAPLAB_ROS_COMMON_GFLAGS_INTERFACE_H_
16 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <package format="2">
 3 |   <name>maplab_ros_common</name>
 4 |   <version>2.2.0</version>
 5 |   <description>Common ROS tools.</description>
 6 |   <maintainer email="maplab-dev@mavt.ethz.ch">maplab-developers</maintainer>
 7 |   <license>Apache 2.0</license>
 8 | 
 9 |   <buildtool_depend>catkin_simple</buildtool_depend>
10 |   <buildtool_depend>catkin</buildtool_depend>
11 | 
12 |   <depend>cv_bridge</depend>
13 |   <depend>gflags_catkin</depend>
14 |   <depend>glog_catkin</depend>
15 |   <depend>rosbag</depend>
16 |   <depend>roscpp</depend>
17 | </package>
18 | 


--------------------------------------------------------------------------------
/hough2map/src/node.cpp:
--------------------------------------------------------------------------------
 1 | #include "hough2map/detector.h"
 2 | #include <maplab-ros-common/gflags-interface.h>
 3 | #include <ros/ros.h>
 4 | 
 5 | int main(int argc, char *argv[]) {
 6 |   google::InitGoogleLogging(argv[0]);
 7 |   google::ParseCommandLineFlags(&argc, &argv, true);
 8 |   google::InstallFailureSignalHandler();
 9 |   FLAGS_alsologtostderr = true;
10 |   FLAGS_colorlogtostderr = true;
11 | 
12 |   ros::init(argc, argv, "hough2map");
13 |   ros::NodeHandle nh, nh_private("~");
14 | 
15 |   // Reading RosParams and storing as gflags.
16 |   ros_common::parseGflagsFromRosParams(argv[0], nh_private);
17 | 
18 |   // Create pole detector.
19 |   ROS_INFO("Using Hough detector.");
20 | 
21 |   hough2map::Detector detector(nh, nh_private);
22 | 
23 |   ros::spin();
24 | 
25 |   return 0;
26 | }
27 | 


--------------------------------------------------------------------------------
/hough2map/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(hough2map)
 3 | 
 4 | find_package(catkin_simple REQUIRED)
 5 | catkin_simple(ALL_DEPS_REQUIRED)
 6 | 
 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall")
 8 | 
 9 | find_package(OpenMP)
10 | 
11 | 
12 | cs_add_executable(${PROJECT_NAME}
13 |   src/node.cpp
14 | )
15 | 
16 | cs_add_library(${PROJECT_NAME}_lib
17 |   src/detector.cpp
18 | )
19 | 
20 | if(OpenMP_CXX_FOUND)
21 |     target_link_libraries(hough2map PUBLIC OpenMP::OpenMP_CXX)
22 | endif()
23 | if (OPENMP_FOUND)
24 |     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
25 |     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
26 |     set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
27 | endif()
28 | 
29 | target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib -fopenmp)
30 | 
31 | cs_install()
32 | cs_export()
33 | 


--------------------------------------------------------------------------------
/hough2map/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>hough2map</name>
 4 |   <version>0.0.0</version>
 5 |   <description>Hough2Map --  Iterative Event-based Hough Transform for High-Speed Railway Mapping</description>
 6 |   <maintainer email="florian.tschopp@mavt.ethz.ch">Florian Tschopp</maintainer>
 7 |   <maintainer email="andrei.cramariuc@mavt.ethz.ch">Andrei Cramariuc</maintainer>
 8 |   <maintainer email="cornelius.voneinem@mavt.ethz.ch">Cornelius von Einem</maintainer>
 9 |   <license>Apache 2.0</license>
10 | 
11 |   <buildtool_depend>catkin</buildtool_depend>
12 |   <buildtool_depend>catkin_simple</buildtool_depend>
13 | 
14 |   <depend>roscpp</depend>
15 | 
16 |   <depend>std_msgs</depend>
17 |   <depend>sensor_msgs</depend>
18 |   <depend>dvs_msgs</depend>
19 |   <depend>custom_msgs</depend>
20 | 
21 |   <depend>maplab_ros_common</depend>
22 |   <depend>eigen_catkin</depend>
23 |   <depend>opencv3_catkin</depend>
24 |   <depend>cv_bridge</depend>
25 |   <depend>glog_catkin</depend>
26 |   <depend>gflags_catkin</depend>
27 |   <depend>dvs_renderer</depend>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
 1 | [submodule "dependencies/rpg_dvs_ros"]
 2 | 	path = dependencies/rpg_dvs_ros
 3 | 	url = https://github.com/uzh-rpg/rpg_dvs_ros
 4 | [submodule "dependencies/drivers"]
 5 | 	path = dependencies/drivers
 6 | 	url = https://gitlab.com/autowarefoundation/autoware.ai/drivers.git
 7 | [submodule "dependencies/opencv3_catkin"]
 8 | 	path = dependencies/opencv3_catkin
 9 | 	url = https://github.com/ethz-asl/opencv3_catkin.git
10 | [submodule "dependencies/eigen_catkin"]
11 | 	path = dependencies/eigen_catkin
12 | 	url = https://github.com/ethz-asl/eigen_catkin
13 | [submodule "dependencies/glog_catkin"]
14 | 	path = dependencies/glog_catkin
15 | 	url = https://github.com/ethz-asl/glog_catkin
16 | [submodule "dependencies/gflags_catkin"]
17 | 	path = dependencies/gflags_catkin
18 | 	url = https://github.com/ethz-asl/gflags_catkin
19 | [submodule "dependencies/catkin_simple"]
20 | 	path = dependencies/catkin_simple
21 | 	url = https://github.com/catkin/catkin_simple.git
22 | [submodule "dependencies/vision_opencv"]
23 | 	path = dependencies/vision_opencv
24 | 	url = https://github.com/ros-perception/vision_opencv.git
25 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/include/maplab-ros-common/image-conversion.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPLAB_ROS_COMMON_IMAGE_CONVERSION_H_
 2 | #define MAPLAB_ROS_COMMON_IMAGE_CONVERSION_H_
 3 | 
 4 | #include <string>
 5 | 
 6 | #include <cv_bridge/cv_bridge.h>
 7 | #include <glog/logging.h>
 8 | #include <opencv2/core.hpp>
 9 | #include <sensor_msgs/Image.h>
10 | 
11 | namespace ros_common {
12 | 
13 | inline void imageMsgToCvMat(
14 |     const sensor_msgs::Image& image_msg, const std::string image_encoding,
15 |     cv::Mat* cv_mat) {
16 |   CHECK(!image_encoding.empty());
17 |   CHECK_NOTNULL(cv_mat);
18 |   cv_bridge::CvImageConstPtr cv_bridge_image;
19 |   try {
20 |     cv_bridge_image = cv_bridge::toCvCopy(image_msg, image_encoding);
21 |   } catch (const cv_bridge::Exception& e) {
22 |     LOG(FATAL) << "cv_bridge exception: " << e.what();
23 |   }
24 |   CHECK(cv_bridge_image);
25 |   *cv_mat = cv_bridge_image->image;
26 | }
27 | 
28 | inline void imageMsgToCvMat(
29 |     const sensor_msgs::Image& image_msg, cv::Mat* cv_mat) {
30 |   CHECK_NOTNULL(cv_mat);
31 |   const std::string& image_encoding = image_msg.encoding;
32 |   imageMsgToCvMat(image_msg, image_encoding, cv_mat);
33 | }
34 | 
35 | }  // namespace ros_common
36 | 
37 | #endif  // MAPLAB_ROS_COMMON_IMAGE_CONVERSION_H_
38 | 


--------------------------------------------------------------------------------
/hough2map/launch/hough2map.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- rosbag playback -->
 3 |   <arg name="bag" />
 4 |   <node pkg="rosbag" type="play" name="rosbag" args="$(arg bag) -q -d 3 -s 0"/>
 5 | 
 6 |   <!-- mapping pipeline -->
 7 |   <node name="hough2map" pkg="hough2map" type="hough2map" output="screen">
 8 |     <param name="camera_offset_x" value="2.055"/>
 9 |     <param name="camera_offset_y" value="0.87"/>
10 |     <param name="buffer_size_s" value="260"/>
11 |     <param name="hough_2_min_threshold" value="7000"/>
12 |     <param name="display_2nd_hough_space" value="true"/>
13 |     <param name="map_output" value="true"/>
14 |     <param name="show_lines_in_video" value="true"/>
15 |     <param name="odometry_event_alignment" value="-0.314897455547225"/>
16 |   </node>
17 | 
18 |   <!-- event visualization -->
19 |   <node name="dvs_renderer_events" pkg="dvs_renderer" type="dvs_renderer">
20 |     <param name="display_method" value="red-blue"/>
21 |     <param name="num_event_msgs" value="1"/>
22 |     <remap from="events" to="/dvs/events" />
23 |     <remap from="image" to="/dvs/image_raw" />
24 |     <remap from="camera_info" to="/dvs/camera_info" />
25 |   </node>
26 | 
27 |   <!-- display -->
28 |   <node name="image_view" pkg="rqt_image_view" type="rqt_image_view">
29 |     <remap from="image" to="dvs_rendering"/>
30 |   </node>
31 | 
32 | 
33 | </launch>
34 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/include/maplab-ros-common/rosbag-tools.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPLAB_ROS_COMMON_ROSBAG_TOOLS_H_
 2 | #define MAPLAB_ROS_COMMON_ROSBAG_TOOLS_H_
 3 | 
 4 | #include <functional>
 5 | #include <string>
 6 | 
 7 | #include <rosbag/message_instance.h>
 8 | 
 9 | namespace ros_common {
10 | 
11 | typedef std::function<void(const rosbag::MessageInstance&)>
12 |     MessageInstanceCallback;
13 | 
14 | // Iterates through all messages in the rosbag and invokes the given callback
15 | // for all of them.
16 | void forEachMessageInBag(
17 |     const std::string& bag_name, const MessageInstanceCallback& callback);
18 | 
19 | template <typename MessageType>
20 | using MessageCallback = std::function<void(
21 |     const uint64_t timestamp_nsec, const std::string& topic_name,
22 |     const MessageType& message)>;
23 | 
24 | // Iterates through all messages in the rosbag and invokes the given callback
25 | // for all messages with a matching message type.
26 | template <typename MessageType>
27 | void forSpecifiedMessageTypesInBag(
28 |     const std::string& bag_name, const MessageCallback<MessageType>& callback);
29 | 
30 | template <typename MessageType, typename... OtherMessageTypeCallbacks>
31 | void forSpecifiedMessageTypesInBag(
32 |     const std::string& bag_name, const MessageCallback<MessageType>& callback,
33 |     OtherMessageTypeCallbacks... other_callbacks);
34 | 
35 | }  // namespace ros_common
36 | 
37 | #include "maplab-ros-common/implementation/rosbag-tools.h"
38 | 
39 | #endif  // MAPLAB_ROS_COMMON_ROSBAG_TOOLS_H_
40 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/src/gflags-interface.cc:
--------------------------------------------------------------------------------
 1 | #include "maplab-ros-common/gflags-interface.h"
 2 | 
 3 | #include <gflags/gflags.h>
 4 | #include <glog/logging.h>
 5 | #include <ros/ros.h>
 6 | 
 7 | namespace ros_common {
 8 | 
 9 | void parseGflagsFromRosParams(
10 |     const char* program_name, const ros::NodeHandle& nh_private) {
11 |   std::vector<google::CommandLineFlagInfo> flags;
12 |   google::GetAllFlags(&flags);
13 |   LOG(INFO) << "Parsing gflags from ROS params...";
14 | 
15 |   std::stringstream ss;
16 | 
17 |   for (google::CommandLineFlagInfo& flag : flags) {
18 |     if (flag.type == "int32" || flag.type == "uint64" || flag.type == "int64") {
19 |       int32_t ros_param_value;
20 |       if (nh_private.getParam(flag.name, ros_param_value)) {
21 |         ss << "--" << flag.name << "=" << ros_param_value << std::endl;
22 |       }
23 |     } else if (flag.type == "bool") {
24 |       bool ros_param_value;
25 |       if (nh_private.getParam(flag.name, ros_param_value)) {
26 |         ss << "--" << flag.name << "=" << ros_param_value << std::endl;
27 |       }
28 |     } else if (flag.type == "string") {
29 |       std::string ros_param_value;
30 |       if (nh_private.getParam(flag.name, ros_param_value)) {
31 |         if (ros_param_value.find_first_of("\t\n ") != std::string::npos) {
32 |           LOG(WARNING) << "Cannot parse ROS string parameter '" << flag.name
33 |                        << "' as gflag because it contains whitespaces! value: '"
34 |                        << ros_param_value << "'";
35 |         } else {
36 |           ss << "--" << flag.name << "=" << ros_param_value << std::endl;
37 |         }
38 |       }
39 |     } else if (flag.type == "double") {
40 |       double ros_param_value;
41 |       if (nh_private.getParam(flag.name, ros_param_value)) {
42 |         ss << "--" << flag.name << "=" << ros_param_value << std::endl;
43 |       }
44 |     } else {
45 |       LOG(WARNING) << "Cannot parse gflag '" << flag.name
46 |                    << "' from ROS params, type " << flag.type
47 |                    << " is not supported!";
48 |     }
49 |   }
50 |   const std::string ros_param_gflag_file = ss.str();
51 |   CHECK(google::ReadFlagsFromString(
52 |       ros_param_gflag_file, program_name, false /*errors_are_fatal*/));
53 |   LOG(INFO) << "\n\nThe following Gflags have been set using ROS params:\n"
54 |             << ros_param_gflag_file << "\n";
55 | }
56 | 
57 | }  // namespace ros_common
58 | 


--------------------------------------------------------------------------------
/dependencies/maplab-ros-common/include/maplab-ros-common/implementation/rosbag-tools.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPLAB_ROS_COMMON_IMPLEMENTATION_ROSBAG_TOOLS_H_
 2 | #define MAPLAB_ROS_COMMON_IMPLEMENTATION_ROSBAG_TOOLS_H_
 3 | 
 4 | #include <string>
 5 | 
 6 | #include <boost/shared_ptr.hpp>
 7 | #include <glog/logging.h>
 8 | #include <rosbag/bag.h>
 9 | #include <rosbag/view.h>
10 | 
11 | namespace ros_common {
12 | 
13 | void forEachMessageInBag(
14 |     const std::string& bag_name, const MessageInstanceCallback& callback) {
15 |   CHECK(!bag_name.empty());
16 |   try {
17 |     const rosbag::Bag bag(bag_name, rosbag::bagmode::Read);
18 |     rosbag::View bag_view(bag);
19 |     for (const rosbag::MessageInstance& message_instance : bag_view) {
20 |       callback(message_instance);
21 |     }
22 |   } catch (const std::exception& ex) {
23 |     LOG(FATAL) << "Could not open the rosbag " << bag_name << ": " << ex.what();
24 |   }
25 | }
26 | 
27 | template <typename MessageType>
28 | void invokeCallbackIfMessageIsMatching(
29 |     const rosbag::MessageInstance& message_instance,
30 |     const MessageCallback<MessageType>& callback) {
31 |   CHECK(callback);
32 |   if (message_instance.isType<MessageType>()) {
33 |     const uint64_t timestamp_nsec = message_instance.getTime().toNSec();
34 |     const std::string& topic_name = message_instance.getTopic();
35 |     CHECK(!topic_name.empty());
36 |     const boost::shared_ptr<MessageType> message_ptr =
37 |         message_instance.instantiate<MessageType>();
38 |     CHECK(message_ptr);
39 |     callback(timestamp_nsec, topic_name, *message_ptr.get());
40 |   }
41 | }
42 | 
43 | template <typename MessageType, typename... OtherMessageTypeCallbacks>
44 | void invokeCallbackIfMessageIsMatching(
45 |     const rosbag::MessageInstance& message_instance,
46 |     const MessageCallback<MessageType>& callback,
47 |     OtherMessageTypeCallbacks... other_callbacks) {
48 |   CHECK(callback);
49 |   invokeCallbackIfMessageIsMatching(message_instance, callback);
50 |   invokeCallbackIfMessageIsMatching(message_instance, other_callbacks...);
51 | }
52 | 
53 | template <typename MessageType>
54 | void forSpecifiedMessageTypesInBag(
55 |     const std::string& bag_name, const MessageCallback<MessageType>& callback) {
56 |   CHECK(!bag_name.empty());
57 |   const MessageInstanceCallback message_instance_callback =
58 |       [&](const rosbag::MessageInstance& message_instance) {
59 |         invokeCallbackIfMessageIsMatching(message_instance, callback);
60 |       };
61 |   forEachMessageInBag(bag_name, message_instance_callback);
62 | }
63 | 
64 | template <typename MessageType, typename... OtherMessageTypeCallbacks>
65 | void forSpecifiedMessageTypesInBag(
66 |     const std::string& bag_name, const MessageCallback<MessageType>& callback,
67 |     OtherMessageTypeCallbacks... other_callbacks) {
68 |   CHECK(!bag_name.empty());
69 |   const MessageInstanceCallback message_instance_callback =
70 |       [&](const rosbag::MessageInstance& message_instance) {
71 |         invokeCallbackIfMessageIsMatching(
72 |             message_instance, callback, other_callbacks...);
73 |       };
74 |   forEachMessageInBag(bag_name, message_instance_callback);
75 | }
76 | 
77 | }  // namespace ros_common
78 | 
79 | #endif  // MAPLAB_ROS_COMMON_IMPLEMENTATION_ROSBAG_TOOLS_H_
80 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Hough²Map
 2 | Iterative Event-based Hough Transform for High-Speed Railway Mapping
 3 | 
 4 | Ubuntu 20.04+ROS noetic [![Build Status](https://jenkins.asl.ethz.ch/buildStatus/icon?job=Hough2Map)](https://jenkins.asl.ethz.ch/job/Hough2Map/)
 5 | 
 6 | ## Abstract
 7 | 
 8 | To cope with the growing demand for transportation on the railway system, accurate, robust, and high-frequency positioning is required to enable a safe and efficient utilization of the existing railway infrastructure.
 9 | As a basis for a localization system we propose a complete on-board mapping pipeline able to map robust meaningful landmarks, such as poles from power lines, in the vicinity of the vehicle.
10 | Such poles are good candidates for reliable and long term landmarks even through difficult weather conditions or seasonal changes.
11 | To address the challenges of motion blur and illumination changes in railway scenarios we employ a DVS, a novel event-based camera.
12 | Using a sideways oriented on-board camera, poles appear as vertical lines.
13 | To map such lines in a real-time event stream, we introduce **Hough²Map**, a novel consecutive iterative event-based Hough transform framework capable of detecting, tracking, and triangulating close-by structures.
14 | We demonstrate the mapping reliability and accuracy of **Hough²Map** on real-world data in typical usage scenarios and evaluate using surveyed infrastructure ground truth maps.
15 | **Hough²Map** achieves a detection reliability of up to 92% and a mapping root mean square error accuracy of 1.1518m.
16 | 
17 | 
18 | 
19 | 
20 | ## Paper and Video
21 | 
22 | The **Hough²Map** pipeline is described in the following publication:
23 | 
24 | - Florian Tschopp, Cornelius von Einem, Andrei Cramariuc, David Hug, Andrew William Palmer, Roland Siegwart, Margarita Chli, Juan Nieto, **Hough²Map – Iterative Event-based Hough Transform for High-Speed Railway Mapping**, in _IEEE Robotics and Automation Letters_, April 2021. [[PDF](https://arxiv.org/pdf/2102.08145.pdf)] [[Video](https://www.youtube.com/watch?v=YPSiODVzD-I)]
25 | 
26 | 
27 | ```bibtex
28 | @ARTICLE{Tschopp2021Hough2Map,  
29 |   author={Tschopp, Florian and von Einem, Cornelius and Cramariuc, Andrei and Hug, David and Palmer, Andrew William and Siegwart, Roland and Chli, Margarita and Nieto, Juan},  
30 |   journal={IEEE Robotics and Automation Letters},   
31 |   title={Hough$^2$Map – Iterative Event-Based Hough Transform for High-Speed Railway Mapping},   
32 |   year={2021},  
33 |   volume={6},  
34 |   number={2},  
35 |   pages={2745-2752},  
36 |   doi={10.1109/LRA.2021.3061404}
37 | }
38 | ```
39 | 
40 | Please also have a look at our video:
41 | 
42 | [![Hough²Map Youtube Video](http://img.youtube.com/vi/YPSiODVzD-I/0.jpg)](http://www.youtube.com/watch?v=YPSiODVzD-I)
43 | 
44 | ## Install
45 | 
46 | ### Setup ROS, catkin workspace and system dependencies
47 | Refer to the [install instructions](https://maplab.asl.ethz.ch/docs/develop/pages/installation/A_Installation-Ubuntu.html#manual-installation) of maplab up to the cloning and building of maplab itself.
48 | 
49 | Install additional system dependency:
50 | ```bash
51 | sudo apt install libomp-dev --yes
52 | 
53 | ```
54 | ### Clone and build
55 | Pay special attention to the install instructions of the [rpg_dvs_ros](https://github.com/uzh-rpg/rpg_dvs_ros) package, especially regarding setting the catkin build type:
56 | 
57 | ```bash
58 | cd ~/catkin_ws/src/
59 | catkin config --merge-devel --cmake-args -DCMAKE_BUILD_TYPE=Release
60 | git clone git@github.com:ethz-asl/Hough2Map.git --recursive
61 | catkin build hough2map
62 | ```
63 | 
64 | ### Run
65 | Run using:
66 | ```
67 | roslaunch hough2map hough2map.launch bag:="path to bag file"
68 | ```
69 | 


--------------------------------------------------------------------------------
/hough2map/include/hough2map/detector.h:
--------------------------------------------------------------------------------
  1 | #ifndef HOUGH2MAP_DETECTOR_H_
  2 | #define HOUGH2MAP_DETECTOR_H_
  3 | 
  4 | #include <custom_msgs/orientationEstimate.h>
  5 | #include <custom_msgs/positionEstimate.h>
  6 | #include <custom_msgs/velocityEstimate.h>
  7 | #include <dvs_msgs/Event.h>
  8 | #include <dvs_msgs/EventArray.h>
  9 | #include <geometry_msgs/PoseArray.h>
 10 | #include <ros/ros.h>
 11 | #include <sensor_msgs/Image.h>
 12 | #include <sensor_msgs/image_encodings.h>
 13 | 
 14 | #include <Eigen/Dense>
 15 | #include <boost/align/aligned_allocator.hpp>
 16 | #include <cmath>
 17 | #include <cv_bridge/cv_bridge.h>
 18 | #include <deque>
 19 | #include <fstream>
 20 | #include <gflags/gflags.h>
 21 | #include <glog/logging.h>
 22 | #include <iostream>
 23 | #include <omp.h>
 24 | #include <opencv2/core/eigen.hpp>
 25 | #include <opencv2/highgui/highgui.hpp>
 26 | #include <opencv2/imgproc/imgproc.hpp>
 27 | #include <vector>
 28 | 
 29 | namespace hough2map {
 30 | class Detector {
 31 | public:
 32 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 33 |   Detector(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
 34 |   virtual ~Detector();
 35 | 
 36 | protected:
 37 |   std::string detector_name_ = "Hough";
 38 | 
 39 | private:
 40 |   // Struct for describing a detected line.
 41 |   struct line {
 42 |     int ID;
 43 |     int r;
 44 |     float theta;
 45 |     int theta_idx;
 46 |     double time;
 47 |     bool polarity;
 48 |   };
 49 | 
 50 |   // Struct for describing a tracked pole.
 51 |   struct pole {
 52 |     double rho;
 53 |     double theta;
 54 |     double t_enter;
 55 |     double t_leave;
 56 |     double t_center;
 57 |     double speed;
 58 |     int ID;
 59 |     bool polarity;
 60 |     double pos_x;
 61 |     double pos_y;
 62 |     double first_observed;
 63 |     float weight;
 64 |   };
 65 | 
 66 |   struct utm_coordinate {
 67 |     double x;
 68 |     double y;
 69 |     std::string zone;
 70 |   };
 71 | 
 72 |   // Specifying number of threads.
 73 |   static const int kNumThreads = 4;
 74 | 
 75 |   // File Output.
 76 |   const bool file_output_parameter_logging = true;
 77 | 
 78 |   const char *lines_file_path = "/tmp/lines.txt";
 79 |   std::ofstream lines_file;
 80 | 
 81 |   const char *map_file_path = "/tmp/map.txt";
 82 |   std::ofstream map_file;
 83 | 
 84 |   const char *calibration_file_name = "calibration.yaml";
 85 | 
 86 |   // Timing debugging.
 87 |   double total_events_timing_us;
 88 |   double total_msgs_timing_ms;
 89 |   uint64_t total_events;
 90 |   uint64_t total_msgs;
 91 | 
 92 |   // General Parameters for 1st Hough Transform.
 93 |   static const int kHough1RadiusResolution = 260;
 94 |   static const int kHough1AngularResolution = 21;
 95 |   static const int kHough1MinAngle = -10;
 96 |   static const int kHough1MaxAngle = 10;
 97 | 
 98 |   // Precomputing possible angles and their sin/cos values in order to vectorize
 99 |   // the HT.
100 |   Eigen::VectorXf thetas_1_;
101 |   Eigen::MatrixXf polar_param_mapping_1_;
102 | 
103 |   // Hough transform objects.
104 |   typedef Eigen::Matrix<int, kHough1RadiusResolution, kHough1AngularResolution>
105 |       MatrixHough;
106 | 
107 |   // General parameters for 2nd Hough transform.
108 |   static const int kHough2AngularResolution = 65;
109 |   static const int kHough2MinAngle = 1;
110 |   static const int kHough2MaxAngle = 65;
111 |   Eigen::VectorXd thetas_2_;
112 |   Eigen::MatrixXd polar_param_mapping_2_;
113 | 
114 |   static const int kHough2TimestepsPerMsg = 3;
115 |   static const int kHough2MsgPerWindow = 100;
116 | 
117 |   int camera_resolution_width_;
118 |   int camera_resolution_height_;
119 | 
120 |   const float kAcceptableDistortionRange = 40.0;
121 |   float intrinsics_[4];
122 |   float distortion_coeffs_[4];
123 |   Eigen::MatrixXf undist_map_x_;
124 |   Eigen::MatrixXf undist_map_y_;
125 | 
126 |   // ROS interface.
127 |   // ros::NodeHandle nh_;
128 | 
129 |   ros::NodeHandle nh_;
130 |   ros::NodeHandle nh_private_;
131 |   ros::Publisher feature_pub_;
132 |   ros::Subscriber event_sub_;
133 |   ros::Subscriber image_raw_sub_;
134 | 
135 |   ros::Subscriber GPS_pos_;
136 |   ros::Subscriber GPS_orient_;
137 |   ros::Subscriber GPS_vel_;
138 | 
139 |   /* Function definitions. */
140 | 
141 |   // Callback functions for subscribers.
142 |   void eventCallback(const dvs_msgs::EventArray::ConstPtr &msg);
143 |   void positionCallback(const custom_msgs::positionEstimate msg);
144 |   void velocityCallback(const custom_msgs::velocityEstimate msg);
145 |   void orientationCallback(const custom_msgs::orientationEstimate msg);
146 | 
147 |   // Functions for Hough transform computation.
148 |   hough2map::Detector::line addMaxima(int angle, int rad, double time,
149 |                                       bool pol);
150 |   void addMaximaInRadius(int i, int radius,
151 |                          const MatrixHough &total_hough_space,
152 |                          int local_threshold, bool polarity, double timestamp,
153 |                          std::vector<hough2map::Detector::line> *new_maxima,
154 |                          std::vector<int> *new_maxima_value,
155 |                          bool skip_center = false);
156 |   void applySuppressionRadius(
157 |       const std::vector<hough2map::Detector::line> &new_maxima,
158 |       const std::vector<int> &new_maxima_value,
159 |       std::vector<hough2map::Detector::line> *current_maxima);
160 |   template <typename DerivedVec, typename DerivedMat>
161 |   void initializeSinCosMap(Eigen::EigenBase<DerivedVec> &angles,
162 |                            Eigen::EigenBase<DerivedMat> &sin_cos_map,
163 |                            const int kMinAngle, const int kMaxAngle,
164 |                            const int kNumSteps);
165 |   bool isLocalMaxima(const Eigen::MatrixXi &hough_space, int i, int radius);
166 |   void newPoleDetection(double rho, double theta, double window_time, bool pol);
167 |   void hough2nms(const int i, const int j, const Eigen::MatrixXi &hough_2_space,
168 |                  std::vector<cv::Vec3f> &detections);
169 |   void computeFullHoughTransform(const int time_step,
170 |                                  const int nms_recompute_window,
171 |                                  MatrixHough &total_hough_space_pos,
172 |                                  MatrixHough &total_hough_space_neg,
173 |                                  const Eigen::MatrixXi &radii);
174 |   void updateHoughSpaceVotes(const bool increment, const int event_idx,
175 |                              const bool pol, const Eigen::MatrixXi &radii,
176 |                              MatrixHough &hough_space_pos,
177 |                              MatrixHough &hough_space_neg);
178 |   void computeFullNMS(
179 |       const int time_step, const int nms_recompute_window,
180 |       const MatrixHough &total_hough_space_pos,
181 |       const MatrixHough &total_hough_space_neg,
182 |       std::vector<std::vector<hough2map::Detector::line>> &cur_maxima_list);
183 |   void itterativeNMS(
184 |       const int time_step, const int nms_recompute_window,
185 |       MatrixHough &total_hough_space_pos, MatrixHough &total_hough_space_neg,
186 |       std::vector<std::vector<hough2map::Detector::line>> &cur_maxima_list,
187 |       const Eigen::MatrixXi &radii);
188 |   bool updateIncrementedNMS(
189 |       const double kTimestamp, const bool polarity, const int kAngle,
190 |       const int kRadius, const MatrixHough &hough_space,
191 |       const std::vector<hough2map::Detector::line> &previous_maxima,
192 |       std::vector<int> &discard,
193 |       std::vector<hough2map::Detector::line> &new_maxima,
194 |       std::vector<int> &new_maxima_value);
195 |   void updateDecrementedNMS(
196 |       const double kTimestamp, const bool polarity, const int kAngle,
197 |       const int kRadius, const MatrixHough &hough_space,
198 |       const std::vector<hough2map::Detector::line> &previous_maxima,
199 |       std::vector<int> &discard,
200 |       std::vector<hough2map::Detector::line> &new_maxima,
201 |       std::vector<int> &new_maxima_value);
202 |   void
203 |   secondHoughTransform(const std::vector<std::vector<hough2map::Detector::line>>
204 |                            &cur_maxima_list);
205 |   void eventPreProcessing(const dvs_msgs::EventArray::ConstPtr &orig_msg,
206 |                           Eigen::MatrixXf &points);
207 | 
208 |   // Initialisation functions.
209 |   void computeUndistortionMapping();
210 |   void initializeTransformationMatrices();
211 |   void loadCalibration();
212 | 
213 |   // Odometry processing functions.
214 |   utm_coordinate deg2utm(double la, double lo);
215 |   template <class S, int rows, int cols>
216 |   Eigen::Matrix<S, rows, cols> queryOdometryBuffer(
217 |       const double query_time,
218 |       const std::deque<Eigen::Matrix<S, rows, cols>> &odometry_buffer);
219 | 
220 |   // Visualization functions.
221 |   void drawPolarCorLine(cv::Mat &image_space, float rho, float theta,
222 |                         cv::Scalar color);
223 |   void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
224 |   void visualizeSecondHoughSpace(const std::vector<cv::Vec3f> &kDetectionsPos,
225 |                                  const std::vector<cv::Vec3f> &kDetectionsNeg);
226 |   void visualizeCurrentLineDetections(
227 |       const std::vector<std::vector<hough2map::Detector::line>>
228 |           &cur_maxima_list);
229 | 
230 |   std::deque<
231 |       Eigen::Matrix<int, kHough1RadiusResolution, kHough2TimestepsPerMsg>>
232 |       hough2_queue_pos_;
233 |   std::deque<
234 |       Eigen::Matrix<int, kHough1RadiusResolution, kHough2TimestepsPerMsg>>
235 |       hough2_queue_neg_;
236 | 
237 |   // Events from the previous dvs_msg need to be carried over to start of the
238 |   // Hough computation of the next events. This basically ensures a continous
239 |   // sliding window.
240 |   dvs_msgs::EventArray feature_msg_;
241 | 
242 |   // Odometry.
243 |   std::deque<Eigen::Vector3d> raw_gps_buffer_;
244 |   std::deque<Eigen::Vector3d> velocity_buffer_;
245 |   std::deque<Eigen::Vector2d> orientation_buffer_;
246 | 
247 |   // Transformation matrix (in [m]) between train and sensors for triangulation.
248 |   Eigen::Matrix3d C_camera_train_;
249 |   Eigen::Matrix3d gps_offset_;
250 |   Eigen::Matrix3d camera_train_offset_;
251 | 
252 |   // Storing the current result of the non-max-suppression.
253 |   cv::Mat cur_greyscale_img_;
254 | };
255 | } // namespace hough2map
256 | 
257 | #endif // HOUGH2MAP_DETECTOR_H_
258 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 | Apache License
  2 |                            Version 2.0, January 2004
  3 |                         http://www.apache.org/licenses/
  4 | 
  5 |    TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
  6 | 
  7 |    1. Definitions.
  8 | 
  9 |       "License" shall mean the terms and conditions for use, reproduction,
 10 |       and distribution as defined by Sections 1 through 9 of this document.
 11 | 
 12 |       "Licensor" shall mean the copyright owner or entity authorized by
 13 |       the copyright owner that is granting the License.
 14 | 
 15 |       "Legal Entity" shall mean the union of the acting entity and all
 16 |       other entities that control, are controlled by, or are under common
 17 |       control with that entity. For the purposes of this definition,
 18 |       "control" means (i) the power, direct or indirect, to cause the
 19 |       direction or management of such entity, whether by contract or
 20 |       otherwise, or (ii) ownership of fifty percent (50%) or more of the
 21 |       outstanding shares, or (iii) beneficial ownership of such entity.
 22 | 
 23 |       "You" (or "Your") shall mean an individual or Legal Entity
 24 |       exercising permissions granted by this License.
 25 | 
 26 |       "Source" form shall mean the preferred form for making modifications,
 27 |       including but not limited to software source code, documentation
 28 |       source, and configuration files.
 29 | 
 30 |       "Object" form shall mean any form resulting from mechanical
 31 |       transformation or translation of a Source form, including but
 32 |       not limited to compiled object code, generated documentation,
 33 |       and conversions to other media types.
 34 | 
 35 |       "Work" shall mean the work of authorship, whether in Source or
 36 |       Object form, made available under the License, as indicated by a
 37 |       copyright notice that is included in or attached to the work
 38 |       (an example is provided in the Appendix below).
 39 | 
 40 |       "Derivative Works" shall mean any work, whether in Source or Object
 41 |       form, that is based on (or derived from) the Work and for which the
 42 |       editorial revisions, annotations, elaborations, or other modifications
 43 |       represent, as a whole, an original work of authorship. For the purposes
 44 |       of this License, Derivative Works shall not include works that remain
 45 |       separable from, or merely link (or bind by name) to the interfaces of,
 46 |       the Work and Derivative Works thereof.
 47 | 
 48 |       "Contribution" shall mean any work of authorship, including
 49 |       the original version of the Work and any modifications or additions
 50 |       to that Work or Derivative Works thereof, that is intentionally
 51 |       submitted to Licensor for inclusion in the Work by the copyright owner
 52 |       or by an individual or Legal Entity authorized to submit on behalf of
 53 |       the copyright owner. For the purposes of this definition, "submitted"
 54 |       means any form of electronic, verbal, or written communication sent
 55 |       to the Licensor or its representatives, including but not limited to
 56 |       communication on electronic mailing lists, source code control systems,
 57 |       and issue tracking systems that are managed by, or on behalf of, the
 58 |       Licensor for the purpose of discussing and improving the Work, but
 59 |       excluding communication that is conspicuously marked or otherwise
 60 |       designated in writing by the copyright owner as "Not a Contribution."
 61 | 
 62 |       "Contributor" shall mean Licensor and any individual or Legal Entity
 63 |       on behalf of whom a Contribution has been received by Licensor and
 64 |       subsequently incorporated within the Work.
 65 | 
 66 |    2. Grant of Copyright License. Subject to the terms and conditions of
 67 |       this License, each Contributor hereby grants to You a perpetual,
 68 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 69 |       copyright license to reproduce, prepare Derivative Works of,
 70 |       publicly display, publicly perform, sublicense, and distribute the
 71 |       Work and such Derivative Works in Source or Object form.
 72 | 
 73 |    3. Grant of Patent License. Subject to the terms and conditions of
 74 |       this License, each Contributor hereby grants to You a perpetual,
 75 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 76 |       (except as stated in this section) patent license to make, have made,
 77 |       use, offer to sell, sell, import, and otherwise transfer the Work,
 78 |       where such license applies only to those patent claims licensable
 79 |       by such Contributor that are necessarily infringed by their
 80 |       Contribution(s) alone or by combination of their Contribution(s)
 81 |       with the Work to which such Contribution(s) was submitted. If You
 82 |       institute patent litigation against any entity (including a
 83 |       cross-claim or counterclaim in a lawsuit) alleging that the Work
 84 |       or a Contribution incorporated within the Work constitutes direct
 85 |       or contributory patent infringement, then any patent licenses
 86 |       granted to You under this License for that Work shall terminate
 87 |       as of the date such litigation is filed.
 88 | 
 89 |    4. Redistribution. You may reproduce and distribute copies of the
 90 |       Work or Derivative Works thereof in any medium, with or without
 91 |       modifications, and in Source or Object form, provided that You
 92 |       meet the following conditions:
 93 | 
 94 |       (a) You must give any other recipients of the Work or
 95 |           Derivative Works a copy of this License; and
 96 | 
 97 |       (b) You must cause any modified files to carry prominent notices
 98 |           stating that You changed the files; and
 99 | 
100 |       (c) You must retain, in the Source form of any Derivative Works
101 |           that You distribute, all copyright, patent, trademark, and
102 |           attribution notices from the Source form of the Work,
103 |           excluding those notices that do not pertain to any part of
104 |           the Derivative Works; and
105 | 
106 |       (d) If the Work includes a "NOTICE" text file as part of its
107 |           distribution, then any Derivative Works that You distribute must
108 |           include a readable copy of the attribution notices contained
109 |           within such NOTICE file, excluding those notices that do not
110 |           pertain to any part of the Derivative Works, in at least one
111 |           of the following places: within a NOTICE text file distributed
112 |           as part of the Derivative Works; within the Source form or
113 |           documentation, if provided along with the Derivative Works; or,
114 |           within a display generated by the Derivative Works, if and
115 |           wherever such third-party notices normally appear. The contents
116 |           of the NOTICE file are for informational purposes only and
117 |           do not modify the License. You may add Your own attribution
118 |           notices within Derivative Works that You distribute, alongside
119 |           or as an addendum to the NOTICE text from the Work, provided
120 |           that such additional attribution notices cannot be construed
121 |           as modifying the License.
122 | 
123 |       You may add Your own copyright statement to Your modifications and
124 |       may provide additional or different license terms and conditions
125 |       for use, reproduction, or distribution of Your modifications, or
126 |       for any such Derivative Works as a whole, provided Your use,
127 |       reproduction, and distribution of the Work otherwise complies with
128 |       the conditions stated in this License.
129 | 
130 |    5. Submission of Contributions. Unless You explicitly state otherwise,
131 |       any Contribution intentionally submitted for inclusion in the Work
132 |       by You to the Licensor shall be under the terms and conditions of
133 |       this License, without any additional terms or conditions.
134 |       Notwithstanding the above, nothing herein shall supersede or modify
135 |       the terms of any separate license agreement you may have executed
136 |       with Licensor regarding such Contributions.
137 | 
138 |    6. Trademarks. This License does not grant permission to use the trade
139 |       names, trademarks, service marks, or product names of the Licensor,
140 |       except as required for reasonable and customary use in describing the
141 |       origin of the Work and reproducing the content of the NOTICE file.
142 | 
143 |    7. Disclaimer of Warranty. Unless required by applicable law or
144 |       agreed to in writing, Licensor provides the Work (and each
145 |       Contributor provides its Contributions) on an "AS IS" BASIS,
146 |       WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 |       implied, including, without limitation, any warranties or conditions
148 |       of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 |       PARTICULAR PURPOSE. You are solely responsible for determining the
150 |       appropriateness of using or redistributing the Work and assume any
151 |       risks associated with Your exercise of permissions under this License.
152 | 
153 |    8. Limitation of Liability. In no event and under no legal theory,
154 |       whether in tort (including negligence), contract, or otherwise,
155 |       unless required by applicable law (such as deliberate and grossly
156 |       negligent acts) or agreed to in writing, shall any Contributor be
157 |       liable to You for damages, including any direct, indirect, special,
158 |       incidental, or consequential damages of any character arising as a
159 |       result of this License or out of the use or inability to use the
160 |       Work (including but not limited to damages for loss of goodwill,
161 |       work stoppage, computer failure or malfunction, or any and all
162 |       other commercial damages or losses), even if such Contributor
163 |       has been advised of the possibility of such damages.
164 | 
165 |    9. Accepting Warranty or Additional Liability. While redistributing
166 |       the Work or Derivative Works thereof, You may choose to offer,
167 |       and charge a fee for, acceptance of support, warranty, indemnity,
168 |       or other liability obligations and/or rights consistent with this
169 |       License. However, in accepting such obligations, You may act only
170 |       on Your own behalf and on Your sole responsibility, not on behalf
171 |       of any other Contributor, and only if You agree to indemnify,
172 |       defend, and hold each Contributor harmless for any liability
173 |       incurred by, or claims asserted against, such Contributor by reason
174 |       of your accepting any such warranty or additional liability.
175 | 
176 |    END OF TERMS AND CONDITIONS
177 | 
178 |    APPENDIX: How to apply the Apache License to your work.
179 | 
180 |       To apply the Apache License to your work, attach the following
181 |       boilerplate notice, with the fields enclosed by brackets "{}"
182 |       replaced with your own identifying information. (Don't include
183 |       the brackets!)  The text should be enclosed in the appropriate
184 |       comment syntax for the file format. We also recommend that a
185 |       file or class name and description of purpose be included on the
186 |       same "printed page" as the copyright notice for easier
187 |       identification within third-party archives.
188 | 
189 |    Copyright 2017 Autonomous Systems Lab, ETH Zurich, Switzerland
190 | 
191 |    Licensed under the Apache License, Version 2.0 (the "License");
192 |    you may not use this file except in compliance with the License.
193 |    You may obtain a copy of the License at
194 | 
195 |        http://www.apache.org/licenses/LICENSE-2.0
196 | 
197 |    Unless required by applicable law or agreed to in writing, software
198 |    distributed under the License is distributed on an "AS IS" BASIS,
199 |    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 |    See the License for the specific language governing permissions and
201 |    limitations under the License.


--------------------------------------------------------------------------------
/hough2map/src/detector.cpp:
--------------------------------------------------------------------------------
   1 | #include <chrono>
   2 | #include <omp.h>
   3 | #include <ros/package.h>
   4 | #include <thread>
   5 | 
   6 | #include "hough2map/detector.h"
   7 | 
   8 | DEFINE_int32(hough_1_threshold, 15,
   9 |              "Threshold for the first level Hough transform.");
  10 | DEFINE_int32(hough_1_window_size, 300,
  11 |              "Max queue length for the first Hough transform.");
  12 | DEFINE_int32(
  13 |     hough_space_NMS_suppression_radius, 10,
  14 |     "Non-Maximum-Suppression suppression radius to enforce Maxima separation");
  15 | DEFINE_int32(hough_2_min_threshold, 7000,
  16 |              "Minimal Threshold for the second Hough transform");
  17 | DEFINE_int32(event_subsample_factor, 1,
  18 |              "Subsample Events by a constant factor");
  19 | DEFINE_bool(show_lines_in_video, false,
  20 |             "Plot detected lines in the video stream");
  21 | DEFINE_bool(lines_output, false, "Output detected lines to a file");
  22 | DEFINE_bool(map_output, false, "Export detected poles to file");
  23 | DEFINE_bool(display_2nd_hough_space, false,
  24 |             "Display the current 2nd degree hough Space");
  25 | DEFINE_bool(odometry_available, true, "A GPS Odometry is available");
  26 | DEFINE_double(odometry_event_alignment, 0,
  27 |               "Manual time synchronization to compensate misalignment between "
  28 |               "odometry and DVS timestamps");
  29 | DEFINE_double(camera_offset_x, 0, "Camera offset along the train axis");
  30 | DEFINE_double(camera_offset_y, 0,
  31 |               "Camera offset perpendicular to the left of the train axis");
  32 | DEFINE_bool(perform_camera_undistortion, true,
  33 |             "Undistort event data according to camera calibration");
  34 | DEFINE_double(buffer_size_s, 30, "Size of the odometry buffer in seconds");
  35 | DEFINE_double(event_array_frequency, 30.0,
  36 |               "Expected frequency of event arrays.");
  37 | DEFINE_double(hough_2_nms_min_angle_separation, 20.0,
  38 |               "The suppression radius in the Non-maximum-suppression for the "
  39 |               "second Hough Transform for angles..");
  40 | DEFINE_double(hough_2_nms_min_rho_separation, 50.0,
  41 |               "The suppression radius in the Non-maximum-suppression for the "
  42 |               "second Hough Transform for pixel separation.");
  43 | DEFINE_double(
  44 |     hough_2_nms_neg_pos_angular_matching, 0.05,
  45 |     "Angular separation between a positive and a negative pole detection for "
  46 |     "them to be confirmed as a pole detection. Value is in radians sqared.");
  47 | DEFINE_double(
  48 |     hough_2_nms_neg_pos_radial_matching, 2500,
  49 |     "Radial separation between a positive and a negative pole detection for "
  50 |     "them to be confirmed as a pole detection. Value is in pixels sqared.");
  51 | 
  52 | namespace hough2map {
  53 | Detector::Detector(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
  54 |     : nh_(nh), nh_private_(nh_private) {
  55 |   // Checking that flags have reasonable values.
  56 |   CHECK_GT(FLAGS_event_array_frequency, 0);
  57 |   CHECK_GE(FLAGS_hough_1_window_size, 1);
  58 |   CHECK_GT(FLAGS_event_subsample_factor, 0);
  59 |   CHECK_GE(FLAGS_buffer_size_s, 1);
  60 |   CHECK_GT(FLAGS_hough_1_threshold, 0);
  61 |   CHECK_GT(FLAGS_hough_2_min_threshold, 0);
  62 | 
  63 |   // File output for the line parameters of the lines in the event stream.
  64 |   if (FLAGS_lines_output) {
  65 |     lines_file.open(lines_file_path);
  66 | 
  67 |     if (lines_file.is_open()) {
  68 |       lines_file << "time,param,pol\n";
  69 |     } else {
  70 |       LOG(FATAL) << "Could not open file:" << lines_file_path << std::endl;
  71 |     }
  72 |   }
  73 | 
  74 |   // Output file for the map data.
  75 |   if (FLAGS_map_output) {
  76 |     map_file.open(map_file_path);
  77 | 
  78 |     if (map_file.is_open()) {
  79 |       map_file << "id,type,time,x,y,orientation,velocity,weight\n";
  80 | 
  81 |     } else {
  82 |       LOG(FATAL) << "Could not open file:" << map_file_path << std::endl;
  83 |     }
  84 |   }
  85 | 
  86 |   // Timing statistics for performance evaluation.
  87 |   total_events_timing_us = 0.0;
  88 |   total_msgs_timing_ms = 0.0;
  89 |   total_events = 0;
  90 |   total_msgs = 0;
  91 | 
  92 |   // Import calibration file.
  93 |   loadCalibration();
  94 | 
  95 |   // Compute undistortion for given camera parameters.
  96 |   computeUndistortionMapping();
  97 | 
  98 |   omp_set_num_threads(kNumThreads);
  99 | 
 100 |   // Various subscribers and publishers for event and odometry data.
 101 |   feature_pub_ = nh_.advertise<dvs_msgs::EventArray>("/feature_events", 1);
 102 |   event_sub_ = nh_.subscribe("/dvs/events", 0, &Detector::eventCallback, this);
 103 |   GPS_pos_ =
 104 |       nh_.subscribe("/oxts/position", 0, &Detector::positionCallback, this);
 105 |   GPS_vel_ =
 106 |       nh_.subscribe("/oxts/velocity", 0, &Detector::velocityCallback, this);
 107 |   GPS_orient_ = nh_.subscribe("/oxts/orientation", 0,
 108 |                               &Detector::orientationCallback, this);
 109 | 
 110 |   // Plot current hough detections in the video.
 111 |   if (FLAGS_show_lines_in_video) {
 112 |     cv::namedWindow("Detected poles", CV_WINDOW_NORMAL);
 113 |     image_raw_sub_ =
 114 |         nh_.subscribe("/dvs/image_raw", 0, &Detector::imageCallback, this);
 115 |   }
 116 | 
 117 |   // Initializig theta, sin and cos values for first and second Hough
 118 |   // transform.
 119 |   initializeSinCosMap(thetas_1_, polar_param_mapping_1_, kHough1MinAngle,
 120 |                       kHough1MaxAngle, kHough1AngularResolution);
 121 |   initializeSinCosMap(thetas_2_, polar_param_mapping_2_, kHough2MinAngle,
 122 |                       kHough2MaxAngle, kHough2AngularResolution);
 123 | 
 124 |   // Initializing various transformation matrizes.
 125 |   initializeTransformationMatrices();
 126 | 
 127 |   if (FLAGS_display_2nd_hough_space) {
 128 |     cv::namedWindow("Hough Transform #2", CV_WINDOW_NORMAL);
 129 |   }
 130 | }
 131 | 
 132 | Detector::~Detector() {
 133 |   // Close all open files.
 134 |   if (FLAGS_lines_output) {
 135 |     lines_file.close();
 136 |   }
 137 | 
 138 |   if (FLAGS_map_output) {
 139 |     map_file.close();
 140 |   }
 141 | }
 142 | 
 143 | const int Detector::kHough1AngularResolution;
 144 | const int Detector::kHough1RadiusResolution;
 145 | 
 146 | void Detector::initializeTransformationMatrices() {
 147 | 
 148 |   // Initialize transformation matrices.
 149 | 
 150 |   // Rotating camera relative to train.
 151 |   C_camera_train_ << 0, -1, 0, 1, 0, 0, 0, 0, 1;
 152 | 
 153 |   // GPS offset to center of train in meters.
 154 |   gps_offset_ << 1, 0, 0, 0, 1, -0.8, 0, 0, 1;
 155 | 
 156 |   // Camera offset to center of train in meters.
 157 |   camera_train_offset_ << 1, 0, -FLAGS_camera_offset_x, 0, 1,
 158 |       FLAGS_camera_offset_y, 0, 0, 1;
 159 | }
 160 | 
 161 | // Function to precompute angles, sin and cos values for a vectorized version
 162 | // of the HT. Templated to deal with float and double accuracy.
 163 | template <typename DerivedVec, typename DerivedMat>
 164 | void Detector::initializeSinCosMap(Eigen::EigenBase<DerivedVec> &angles,
 165 |                                    Eigen::EigenBase<DerivedMat> &sin_cos_map,
 166 |                                    const int kMinAngle, const int kMaxAngle,
 167 |                                    const int kNumSteps) {
 168 | 
 169 |   // Computing the angular step size.
 170 |   CHECK_GT(kNumSteps - 1, 0);
 171 |   const double kDeltaTheta = (kMaxAngle - kMinAngle) / (kNumSteps - 1);
 172 |   // Resizing the respective output matrizes.
 173 |   angles.derived().resize(kNumSteps, 1);
 174 |   sin_cos_map.derived().resize(kNumSteps, 2);
 175 | 
 176 |   // Populating matrizes with angles and sin, cos values.
 177 |   for (int i = 0; i < kNumSteps; i++) {
 178 |     angles.derived()(i) = M_PI * (kMinAngle + i * kDeltaTheta) / 180.0;
 179 |     sin_cos_map.derived()(i, 0) = cos(angles.derived()(i));
 180 |     sin_cos_map.derived()(i, 1) = sin(angles.derived()(i));
 181 |   }
 182 | }
 183 | 
 184 | void Detector::loadCalibration() {
 185 |   // File path to calibration file.
 186 |   std::string package_path = ros::package::getPath("hough2map");
 187 |   std::string calibration_file =
 188 |       package_path + "/share/" + calibration_file_name;
 189 | 
 190 |   cv::FileStorage fs(calibration_file, cv::FileStorage::READ);
 191 | 
 192 |   if (!fs.isOpened()) {
 193 |     LOG(FATAL) << "Could not open calibration file:" << calibration_file
 194 |                << std::endl;
 195 |   }
 196 | 
 197 |   // Import parameters from calibration file.
 198 |   cv::FileNode cam = fs["cam0"];
 199 | 
 200 |   // First, let's import the sensor resolution.
 201 |   cv::FileNode resolution = cam["resolution"];
 202 |   CHECK_EQ(resolution.size(), 2)
 203 |       << ": Not enough calibration data regarding sensor size!";
 204 | 
 205 |   // Importing sensor resolution.
 206 |   camera_resolution_width_ = resolution[0];
 207 |   camera_resolution_height_ = resolution[1];
 208 | 
 209 |   // Importing camera intrinsics. Expecting 4 values.
 210 |   CHECK_EQ(cam["intrinsics"].size(), 4)
 211 |       << ": Not enough calibration data regarding sensor intrinsics!";
 212 |   cv::FileNodeIterator it = cam["intrinsics"].begin(),
 213 |                        it_end = cam["intrinsics"].end();
 214 |   int i = 0;
 215 |   for (; it != it_end; ++it) {
 216 |     intrinsics_[i] = (*it).real();
 217 |     i++;
 218 |   }
 219 | 
 220 |   // Importing the distortion coefficients, again expecting 4 values.
 221 |   CHECK_EQ(cam["distortion_coeffs"].size(), 4)
 222 |       << ": Not enough calibration data regarding distortion coefficients!";
 223 |   it = cam["distortion_coeffs"].begin(),
 224 |   it_end = cam["distortion_coeffs"].end();
 225 |   i = 0;
 226 |   for (; it != it_end; ++it) {
 227 |     distortion_coeffs_[i] = (*it).real();
 228 |     i++;
 229 |   }
 230 | }
 231 | 
 232 | void Detector::computeUndistortionMapping() {
 233 |   // Setup camera intrinsics from calibration file.
 234 |   cv::Mat camera_matrix = (cv::Mat1d(3, 3) << intrinsics_[0], 0, intrinsics_[2],
 235 |                            0, intrinsics_[1], intrinsics_[3], 0, 0, 1);
 236 |   cv::Mat distortionCoefficients =
 237 |       (cv::Mat1d(1, 4) << distortion_coeffs_[0], distortion_coeffs_[1],
 238 |        distortion_coeffs_[2], distortion_coeffs_[3]);
 239 | 
 240 |   undist_map_x_.resize(camera_resolution_height_, camera_resolution_width_);
 241 |   undist_map_y_.resize(camera_resolution_height_, camera_resolution_width_);
 242 | 
 243 |   // Compute undistortion mapping.
 244 |   for (int i = 0; i < camera_resolution_width_; i++) {
 245 |     for (int j = 0; j < camera_resolution_height_; j++) {
 246 |       cv::Mat_<cv::Point2f> points(1, 1);
 247 |       points(0) = cv::Point2f(i, j);
 248 |       cv::Mat dst;
 249 | 
 250 |       cv::undistortPoints(points, dst, camera_matrix, distortionCoefficients);
 251 |       const float u = intrinsics_[0] * dst.at<float>(0, 0) + intrinsics_[2];
 252 |       const float v = intrinsics_[1] * dst.at<float>(0, 1) + intrinsics_[3];
 253 | 
 254 |       CHECK_GT(u, 0.0 - kAcceptableDistortionRange)
 255 |           << "Horizontal undistortion is larger than expected";
 256 |       CHECK_LT(u, camera_resolution_width_ + kAcceptableDistortionRange)
 257 |           << "Horizontal undistortion is larger than expected";
 258 |       CHECK_GT(v, 0.0 - kAcceptableDistortionRange)
 259 |           << "Vertical undistortion is larger than expected";
 260 |       CHECK_LT(v, camera_resolution_height_ + kAcceptableDistortionRange)
 261 |           << "Vertical undistortion is larger than expected";
 262 | 
 263 |       undist_map_x_(j, i) = u;
 264 |       undist_map_y_(j, i) = v;
 265 |     }
 266 |   }
 267 | }
 268 | 
 269 | // Processing incoming GPS position data.
 270 | void Detector::positionCallback(const custom_msgs::positionEstimate msg) {
 271 |   utm_coordinate current_position_utm = deg2utm(msg.latitude, msg.longitude);
 272 | 
 273 |   // Add raw_gps to current raw_gps buffer (with alighment for manual odometry
 274 |   // to DVS synchronization).
 275 |   const double kAlignedTimestamp =
 276 |       msg.header.stamp.toSec() + FLAGS_odometry_event_alignment;
 277 |   Eigen::Vector3d current_position_txy;
 278 |   current_position_txy[0] = kAlignedTimestamp;
 279 |   current_position_txy[1] = current_position_utm.x;
 280 |   current_position_txy[2] = current_position_utm.y;
 281 |   raw_gps_buffer_.push_back(current_position_txy);
 282 | 
 283 |   while (kAlignedTimestamp - raw_gps_buffer_.front()[0] > FLAGS_buffer_size_s) {
 284 |     raw_gps_buffer_.pop_front();
 285 |   }
 286 | }
 287 | 
 288 | // Processing velocity data and storing in buffer.
 289 | void Detector::velocityCallback(const custom_msgs::velocityEstimate msg) {
 290 | 
 291 |   // Manually synchronizing DVS to odometry timestamps.
 292 |   const double kAlignedTimestamp =
 293 |       msg.header.stamp.toSec() + FLAGS_odometry_event_alignment;
 294 |   // Add velocity to current velocity buffer.
 295 |   Eigen::Vector3d current_velocity_ten;
 296 |   current_velocity_ten[0] = kAlignedTimestamp;
 297 |   current_velocity_ten[1] = msg.velE;
 298 |   current_velocity_ten[2] = msg.velN;
 299 |   velocity_buffer_.push_back(current_velocity_ten);
 300 | 
 301 |   while (kAlignedTimestamp - velocity_buffer_.front()[0] >
 302 |          FLAGS_buffer_size_s) {
 303 |     velocity_buffer_.pop_front();
 304 |   }
 305 | }
 306 | 
 307 | void Detector::orientationCallback(const custom_msgs::orientationEstimate msg) {
 308 | 
 309 |   double yaw = msg.yaw * M_PI / 180.0;
 310 | 
 311 |   // Add orientation to current orientation buffer with manual time stamp
 312 |   // synchronization.
 313 |   const double kAlignedTimestamp =
 314 |       msg.header.stamp.toSec() + FLAGS_odometry_event_alignment;
 315 |   Eigen::Vector2d cur_orient;
 316 |   cur_orient[0] = kAlignedTimestamp;
 317 |   cur_orient[1] = yaw;
 318 |   orientation_buffer_.push_back(cur_orient);
 319 | 
 320 |   while (kAlignedTimestamp - orientation_buffer_.front()[0] >
 321 |          FLAGS_buffer_size_s) {
 322 |     orientation_buffer_.pop_front();
 323 |   }
 324 | }
 325 | 
 326 | void Detector::eventCallback(const dvs_msgs::EventArray::ConstPtr &msg) {
 327 |   const auto kStartTime = std::chrono::high_resolution_clock::now();
 328 | 
 329 |   feature_msg_.header = msg->header;
 330 |   feature_msg_.width = msg->width;
 331 |   feature_msg_.height = msg->height;
 332 | 
 333 |   int num_events = msg->events.size();
 334 |   CHECK_GE(num_events, 1);
 335 | 
 336 |   // If initialized then make sure the last FLAGS_hough_1_window_size events
 337 |   // are prepended to the current list of events and remove older events.
 338 |   if (feature_msg_.events.size() > FLAGS_hough_1_window_size) {
 339 |     std::copy(feature_msg_.events.end() - FLAGS_hough_1_window_size,
 340 |               feature_msg_.events.end(), feature_msg_.events.begin());
 341 |     feature_msg_.events.resize(FLAGS_hough_1_window_size);
 342 |   }
 343 | 
 344 |   // Reshaping the event array into an Eigen matrix.
 345 |   Eigen::MatrixXf points;
 346 |   eventPreProcessing(msg, points);
 347 | 
 348 |   // Number of events after filtering and subsampling.
 349 |   num_events = feature_msg_.events.size();
 350 |   CHECK_GE(num_events, 1);
 351 | 
 352 |   // Check there are enough events for our window size. This is only relevant
 353 |   // during initialization.
 354 |   if (num_events <= FLAGS_hough_1_window_size) {
 355 |     return;
 356 |   }
 357 | 
 358 |   // Computing all the radii for each theta hypothesis. This parameter pair
 359 |   // forms the Hough space. This is done all at once for each event.
 360 |   Eigen::MatrixXi radii;
 361 |   radii.resize(kHough1AngularResolution, num_events);
 362 |   radii = (polar_param_mapping_1_ * points).cast<int>();
 363 | 
 364 |   // Total Hough Space at NMS Intervals
 365 |   // kNmsBatchCount is the reduced number of iterations. This is basically
 366 |   // the number of sub-batches that will be processed in parallel
 367 |   CHECK_GE(kNumThreads, 1);
 368 |   int nms_recompute_window =
 369 |       std::ceil(float(num_events - FLAGS_hough_1_window_size) / kNumThreads);
 370 |   nms_recompute_window =
 371 |       std::max(nms_recompute_window, FLAGS_hough_1_window_size);
 372 |   CHECK_GT(nms_recompute_window, 0);
 373 |   const int kNmsBatchCount = std::ceil(
 374 |       float(num_events - FLAGS_hough_1_window_size) / nms_recompute_window);
 375 | 
 376 |   // Initializing total Hough spaces. Total means the Hough Space for a full
 377 |   // current window, rather than the Hough Space of an individual event. It is
 378 |   // therefore the sum of all the Hough Spaces of the events in the current
 379 |   // window.
 380 |   std::vector<MatrixHough> total_hough_spaces_pos(kNmsBatchCount);
 381 |   std::vector<MatrixHough> total_hough_spaces_neg(kNmsBatchCount);
 382 | 
 383 |   // At this point we are starting the parallelisation scheme of this
 384 |   // pipeline. As events have to be processed sequentially, the sequence is
 385 |   // split into parts. The beginning of each part depends on the end of the
 386 |   // previous one. This beginning state is computed using a full HT and full
 387 |   // NMS.
 388 | 
 389 |   // Resetting the accumulator cells of all Hough spaces for the beginning of
 390 |   // all batches.
 391 | #pragma omp parallel for
 392 |   for (int i = 0; i < kNmsBatchCount; i++) {
 393 |     total_hough_spaces_pos[i].setZero();
 394 |     total_hough_spaces_neg[i].setZero();
 395 |   }
 396 | 
 397 |   // Computing total Hough space every N steps, so for the beginning of each
 398 |   // parallelisation batch. This depends on the last FLAGS_hough_1_window_size
 399 |   // of the previous batch.
 400 | #pragma omp parallel for
 401 |   for (int i = 0; i < kNmsBatchCount; i++) {
 402 |     computeFullHoughTransform(i, nms_recompute_window,
 403 |                               total_hough_spaces_pos[i],
 404 |                               total_hough_spaces_neg[i], radii);
 405 |   }
 406 | 
 407 |   // Each event is treated as a timestep. For each of these timesteps we keep
 408 |   // the active set of maxima in the Hough Space. These are basically the line
 409 |   // detections at each timestep. This whole storage is pre-initialized to
 410 |   // make it ready for parallelizing the whole process.
 411 |   std::vector<std::vector<hough2map::Detector::line>> maxima_list(num_events);
 412 | 
 413 |   // As we compute a full HT at the beginning of each batch, we also need a
 414 |   // full NMS. This is computed here.
 415 | #pragma omp parallel for
 416 |   for (int k = 0; k < kNmsBatchCount; k++) {
 417 |     computeFullNMS(k, nms_recompute_window, total_hough_spaces_pos[k],
 418 |                    total_hough_spaces_neg[k], maxima_list);
 419 |   }
 420 | 
 421 |   // Within each parallelised NMS batch, we can now perform the rest of the
 422 |   // computations iterativels, processing the events in their correct
 423 |   // sequence. This is done in parallel for all batches.
 424 | #pragma omp parallel for
 425 |   for (int k = 0; k < kNmsBatchCount; k++) {
 426 |     itterativeNMS(k, nms_recompute_window, total_hough_spaces_pos[k],
 427 |                   total_hough_spaces_neg[k], maxima_list, radii);
 428 |   }
 429 |   // If visualizations are turned on display them in the video stream.
 430 |   if (FLAGS_show_lines_in_video) {
 431 |     visualizeCurrentLineDetections(maxima_list);
 432 |   }
 433 | 
 434 |   // Run the second Hough Transform for spatio-temporal tracking.
 435 |   secondHoughTransform(maxima_list);
 436 | 
 437 |   // Publish events that were part of the Hough transform (because they were
 438 |   // not filtered out).
 439 |   feature_pub_.publish(feature_msg_);
 440 | 
 441 |   if (num_events > 0) {
 442 |     std::chrono::duration<double, std::micro> duration_us =
 443 |         std::chrono::high_resolution_clock::now() - kStartTime;
 444 | 
 445 |     total_events_timing_us += duration_us.count();
 446 |     total_msgs_timing_ms += duration_us.count() / 1000.0;
 447 | 
 448 |     total_events += num_events;
 449 |     total_msgs++;
 450 | 
 451 |     LOG(INFO) << detector_name_ << std::fixed << std::setprecision(2)
 452 |               << std::setfill(' ') << " speed: " << std::setw(6)
 453 |               << total_events_timing_us / total_events << " us/event | "
 454 |               << std::setw(6) << total_msgs_timing_ms / total_msgs
 455 |               << " ms/msg | " << std::setw(6) << num_events << " e/msg";
 456 |   }
 457 | }
 458 | 
 459 | // Visualizing the current line detections of the first Hough Transform.
 460 | // This function only visualizes vertical lines, for other orientations it needs
 461 | // to be adjusted.
 462 | void Detector::visualizeCurrentLineDetections(
 463 |     const std::vector<std::vector<hough2map::Detector::line>>
 464 |         &cur_maxima_list) {
 465 |   int num_events = feature_msg_.events.size();
 466 | 
 467 |   int positive_detections[camera_resolution_width_] = {0};
 468 |   int negative_detections[camera_resolution_width_] = {0};
 469 | 
 470 |   // Getting the horizontal positions of all vertical line detections.
 471 |   for (int i = 0; i < num_events; i++) {
 472 |     const dvs_msgs::Event &e = feature_msg_.events[i];
 473 |     for (auto &maxima : cur_maxima_list[i]) {
 474 |       if (maxima.polarity) {
 475 |         positive_detections[maxima.r] = 1;
 476 |       } else {
 477 |         negative_detections[maxima.r] = 1;
 478 |       }
 479 |     }
 480 |   }
 481 | 
 482 |   cv::Mat cur_frame = cur_greyscale_img_;
 483 | 
 484 |   // Plottin current line detections.
 485 |   for (int i = 0; i < camera_resolution_width_; i++) {
 486 |     if (positive_detections[i] == 1) {
 487 |       cv::line(cur_frame, cv::Point(i, 0),
 488 |                cv::Point(i, camera_resolution_height_), cv::Scalar(255, 0, 0),
 489 |                2, 8);
 490 |     }
 491 |     if (negative_detections[i] == 1) {
 492 |       cv::line(cur_frame, cv::Point(i, 0),
 493 |                cv::Point(i, camera_resolution_height_), cv::Scalar(0, 0, 255),
 494 |                2, 8);
 495 |     }
 496 |   }
 497 | 
 498 |   cv::imshow("Detected poles", cur_frame);
 499 |   cv::waitKey(1);
 500 | }
 501 | 
 502 | // Performing itterative Non-Maximum suppression on the current batch of
 503 | // events based on a beginning Hough Space.
 504 | void Detector::itterativeNMS(
 505 |     const int time_step, const int nms_recompute_window,
 506 |     MatrixHough &total_hough_space_pos, MatrixHough &total_hough_space_neg,
 507 |     std::vector<std::vector<hough2map::Detector::line>> &cur_maxima_list,
 508 |     const Eigen::MatrixXi &radii) {
 509 | 
 510 |   std::vector<hough2map::Detector::line> new_maxima;
 511 |   std::vector<int> new_maxima_value;
 512 |   int num_events = feature_msg_.events.size();
 513 | 
 514 |   /* Iterative Hough transform */
 515 | 
 516 |   // Itterating over all events which are part of this current batch. These
 517 |   // will be added and removed through the iteration process.
 518 |   const int left =
 519 |       FLAGS_hough_1_window_size + time_step * nms_recompute_window + 1;
 520 |   const int right = std::min(left + nms_recompute_window - 1, num_events);
 521 |   CHECK_GE(right, left);
 522 | 
 523 |   for (int l = left; l < right; l++) {
 524 |     // Getting the event that gets added right now.
 525 |     const dvs_msgs::Event &event = feature_msg_.events[l];
 526 |     const double kTimestamp = event.ts.toSec();
 527 |     CHECK_GE(l - 1, 0);
 528 | 
 529 |     // Establishing the lists of maxima for this timestep and the ones of the
 530 |     // previous timestep
 531 |     std::vector<hough2map::Detector::line> &current_maxima = cur_maxima_list[l];
 532 |     std::vector<hough2map::Detector::line> &previous_maxima =
 533 |         cur_maxima_list[l - 1];
 534 | 
 535 |     // Incrementing the accumulator cells for the current event.
 536 |     updateHoughSpaceVotes(true, l, event.polarity, radii, total_hough_space_pos,
 537 |                           total_hough_space_neg);
 538 | 
 539 |     // Find the oldest event in the current window and get ready to remove it.
 540 |     const int kLRemove = l - FLAGS_hough_1_window_size;
 541 |     CHECK_GE(l - FLAGS_hough_1_window_size, 0);
 542 |     const dvs_msgs::Event &event_remove = feature_msg_.events[kLRemove];
 543 | 
 544 |     // Decrement the accumulator cells for the event to be removed.
 545 |     updateHoughSpaceVotes(false, kLRemove, event_remove.polarity, radii,
 546 |                           total_hough_space_pos, total_hough_space_neg);
 547 | 
 548 |     /* Iterative non-maxima suppression */
 549 | 
 550 |     // The Hough Spaces have been update. Now the iterative NMS has to run and
 551 |     // update the list of known maxima. First reset the temporary lists.
 552 |     new_maxima.clear();
 553 |     new_maxima_value.clear();
 554 | 
 555 |     // Remember which past maxima are no longer maxima so that
 556 |     // we don't need to check again at the end.
 557 |     std::vector<int> discard(previous_maxima.size(), 0);
 558 | 
 559 |     /* Phase 1 - Obtain candidates for global maxima */
 560 | 
 561 |     // For points that got incremented.
 562 |     for (int i = 0; i < kHough1AngularResolution; ++i) {
 563 |       const int kRadius = radii(i, l);
 564 | 
 565 |       if ((kRadius >= 0) && (kRadius < kHough1RadiusResolution)) {
 566 |         if (event.polarity) {
 567 |           bool skip_center = false;
 568 |           updateIncrementedNMS(kTimestamp, event.polarity, i, kRadius,
 569 |                                total_hough_space_pos, previous_maxima, discard,
 570 |                                new_maxima, new_maxima_value);
 571 |           // The center and a neighbour have the same value so
 572 |           // no point in checking if it is a local maximum.
 573 |           if (skip_center) {
 574 |             continue;
 575 |           }
 576 | 
 577 |         } else {
 578 |           bool skip_center = false;
 579 |           updateIncrementedNMS(kTimestamp, event.polarity, i, kRadius,
 580 |                                total_hough_space_neg, previous_maxima, discard,
 581 |                                new_maxima, new_maxima_value);
 582 |           // The center and a neighbour have the same value so
 583 |           // no point in checking if it is a local maximum.
 584 |           if (skip_center) {
 585 |             continue;
 586 |           }
 587 |         }
 588 |       }
 589 |     }
 590 | 
 591 |     // For accumulator cells that got decremented.
 592 |     for (int i = 0; i < kHough1AngularResolution; i++) {
 593 |       const int kRadius = radii(i, kLRemove);
 594 | 
 595 |       if ((kRadius >= 0) && (kRadius < kHough1RadiusResolution)) {
 596 |         if (event_remove.polarity) {
 597 |           updateDecrementedNMS(kTimestamp, event_remove.polarity, i, kRadius,
 598 |                                total_hough_space_pos, previous_maxima, discard,
 599 |                                new_maxima, new_maxima_value);
 600 | 
 601 |         } else {
 602 |           updateDecrementedNMS(kTimestamp, event_remove.polarity, i, kRadius,
 603 |                                total_hough_space_neg, previous_maxima, discard,
 604 |                                new_maxima, new_maxima_value);
 605 |         }
 606 |       }
 607 |     }
 608 | 
 609 |     if (new_maxima.empty()) {
 610 |       // No new maxima in the temporary storage, so we only get rid of the
 611 |       // expired ones and keep the rest as it was unchanged.
 612 |       for (int i = 0; i < previous_maxima.size(); i++) {
 613 |         if (!discard[i]) {
 614 |           current_maxima.push_back(previous_maxima[i]);
 615 |         }
 616 |       }
 617 |     } else {
 618 |       // We discard all expired old maxima and add the ones that are still
 619 |       // valid to the temporary list of new maxima.
 620 |       for (int i = 0; i < previous_maxima.size(); i++) {
 621 |         if (!discard[i]) {
 622 |           const hough2map::Detector::line &kMaximum = previous_maxima[i];
 623 | 
 624 |           new_maxima.push_back(kMaximum);
 625 | 
 626 |           if (kMaximum.polarity) {
 627 |             new_maxima_value.push_back(
 628 |                 total_hough_space_pos(kMaximum.r, kMaximum.theta_idx));
 629 |           } else {
 630 |             new_maxima_value.push_back(
 631 |                 total_hough_space_neg(kMaximum.r, kMaximum.theta_idx));
 632 |           }
 633 |         }
 634 |       }
 635 | 
 636 |       /* Phase 2 - Detect and apply suppression */
 637 | 
 638 |       // Apply the suppression radius.
 639 |       applySuppressionRadius(new_maxima, new_maxima_value, &current_maxima);
 640 | 
 641 |       // We need to check if any of the previous maxima that we
 642 |       // didn't touch have changed. Then we need to check for new
 643 |       // maxima in range and resuppress.
 644 |       while (true) {
 645 |         const int kNumMaximaCandidates = new_maxima.size();
 646 |         for (int i = 0; i < previous_maxima.size(); i++) {
 647 |           if (!discard[i]) {
 648 |             const hough2map::Detector::line &kPreviousMaximum =
 649 |                 previous_maxima[i];
 650 | 
 651 |             bool found = false;
 652 |             for (const hough2map::Detector::line &current_maximum :
 653 |                  current_maxima) {
 654 |               if ((current_maximum.polarity == kPreviousMaximum.polarity) &&
 655 |                   (current_maximum.r == kPreviousMaximum.r) &&
 656 |                   (current_maximum.theta_idx == kPreviousMaximum.theta_idx)) {
 657 |                 found = true;
 658 |                 break;
 659 |               }
 660 |             }
 661 | 
 662 |             if (!found) {
 663 |               discard[i] = true;
 664 | 
 665 |               if (kPreviousMaximum.polarity) {
 666 |                 addMaximaInRadius(kPreviousMaximum.theta_idx,
 667 |                                   kPreviousMaximum.r, total_hough_space_pos,
 668 |                                   FLAGS_hough_1_threshold, true, kTimestamp,
 669 |                                   &new_maxima, &new_maxima_value, true);
 670 |               } else {
 671 |                 addMaximaInRadius(kPreviousMaximum.theta_idx,
 672 |                                   kPreviousMaximum.r, total_hough_space_neg,
 673 |                                   FLAGS_hough_1_threshold, false, kTimestamp,
 674 |                                   &new_maxima, &new_maxima_value, true);
 675 |               }
 676 |             }
 677 |           }
 678 |         }
 679 | 
 680 |         if (kNumMaximaCandidates < new_maxima.size()) {
 681 |           applySuppressionRadius(new_maxima, new_maxima_value, &current_maxima);
 682 |         } else {
 683 |           break;
 684 |         }
 685 |       }
 686 |     }
 687 |   }
 688 | }
 689 | 
 690 | // Updating the iterative Non-Maximum suppression for decremented events.
 691 | void Detector::updateDecrementedNMS(
 692 |     const double kTimestamp, const bool polarity, const int kAngle,
 693 |     const int kRadius, const MatrixHough &hough_space,
 694 |     const std::vector<hough2map::Detector::line> &previous_maxima,
 695 |     std::vector<int> &discard,
 696 |     std::vector<hough2map::Detector::line> &new_maxima,
 697 |     std::vector<int> &new_maxima_value) {
 698 | 
 699 |   // If decremented accumulator cell was previously a maximum, remove it. If
 700 |   // it's still a maximum, we will deal with it later.
 701 |   int k = 0;
 702 |   bool skip_neighborhood = false;
 703 |   for (const hough2map::Detector::line &maximum : previous_maxima) {
 704 |     if ((maximum.polarity == polarity) && (kRadius == maximum.r) &&
 705 |         (kAngle == maximum.theta_idx)) {
 706 |       // Mark as discarded since we will added already
 707 |       // in the next step if it still is above the threshold.
 708 |       discard[k] = true;
 709 | 
 710 |       // Re-add to list of possible maxima for later pruning.
 711 |       addMaximaInRadius(kAngle, kRadius, hough_space, FLAGS_hough_1_threshold,
 712 |                         polarity, kTimestamp, &new_maxima, &new_maxima_value);
 713 | 
 714 |       // The neighborhood of this accumulator cell has been checked as part of
 715 |       // addMaximaInRadius, so no need to do it again.
 716 |       skip_neighborhood = true;
 717 |       break;
 718 |     }
 719 | 
 720 |     k++;
 721 |   }
 722 | 
 723 |   if (!skip_neighborhood) {
 724 |     // Iterate over neighbourhood to check if we might have
 725 |     // created a new local maxima by decreasing.
 726 |     const int m_l = std::max(kAngle - 1, 0);
 727 |     const int m_r = std::min(kAngle + 1, kHough1AngularResolution - 1);
 728 |     const int n_l = std::max(kRadius - 1, 0);
 729 |     const int n_r = std::min(kRadius + 1, kHough1RadiusResolution - 1);
 730 |     for (int m = m_l; m <= m_r; m++) {
 731 |       for (int n = n_l; n <= n_r; n++) {
 732 |         // The center is a separate case.
 733 |         if ((m == kAngle) && (n == kRadius)) {
 734 |           continue;
 735 |         }
 736 | 
 737 |         // Any neighbor points now larger and a maximum?
 738 |         if ((hough_space(kRadius, kAngle) + 1 == hough_space(n, m)) &&
 739 |             (hough_space(n, m) > FLAGS_hough_1_threshold) &&
 740 |             isLocalMaxima(hough_space, m, n)) {
 741 |           // Add to temporary storage.
 742 |           new_maxima.push_back(addMaxima(m, n, kTimestamp, polarity));
 743 |           new_maxima_value.push_back(hough_space(n, m));
 744 |         }
 745 |       }
 746 |     }
 747 |   }
 748 | }
 749 | 
 750 | // Updating the iterative Non-Maximum suppression for incrementing events.
 751 | bool Detector::updateIncrementedNMS(
 752 |     const double kTimestamp, const bool polarity, const int kAngle,
 753 |     const int kRadius, const MatrixHough &hough_space,
 754 |     const std::vector<hough2map::Detector::line> &previous_maxima,
 755 |     std::vector<int> &discard,
 756 |     std::vector<hough2map::Detector::line> &new_maxima,
 757 |     std::vector<int> &new_maxima_value) {
 758 | 
 759 |   // If any of the surrounding ones are equal the center
 760 |   // for sure is not a local maximum.
 761 |   bool skip_center = false;
 762 | 
 763 |   // Iterate over neighbourhood to check if we might have
 764 |   // supressed a surrounding maximum by growing.
 765 |   const int m_l = std::max(kAngle - 1, 0);
 766 |   const int m_r = std::min(kAngle + 1, kHough1AngularResolution - 1);
 767 |   const int n_l = std::max(kRadius - 1, 0);
 768 |   const int n_r = std::min(kRadius + 1, kHough1RadiusResolution - 1);
 769 |   for (int m = m_l; m <= m_r; m++) {
 770 |     for (int n = n_l; n <= n_r; n++) {
 771 |       // The center is a separate case.
 772 |       if ((m == kAngle) && (n == kRadius)) {
 773 |         continue;
 774 |       }
 775 | 
 776 |       // Compare point to its neighbors.
 777 |       if (hough_space(kRadius, kAngle) == hough_space(n, m)) {
 778 |         skip_center = true;
 779 |         // Compare to all known maxima from the previous timestep.
 780 |         int k = 0;
 781 |         for (const hough2map::Detector::line &maximum : previous_maxima) {
 782 |           if ((maximum.polarity == polarity) && (n == maximum.r) &&
 783 |               (m == maximum.theta_idx)) {
 784 |             // We need to discard an old maximum.
 785 |             discard[k] = true;
 786 | 
 787 |             // And add a new one.
 788 |             addMaximaInRadius(m, n, hough_space, FLAGS_hough_1_threshold,
 789 |                               polarity, kTimestamp, &new_maxima,
 790 |                               &new_maxima_value);
 791 |             break;
 792 |           }
 793 | 
 794 |           k++;
 795 |         }
 796 |       }
 797 |     }
 798 |   }
 799 | 
 800 |   // The center and a neighbour have the same value so
 801 |   // no point in checking if it is a local maximum.
 802 |   if (skip_center) {
 803 |     return true;
 804 |   }
 805 | 
 806 |   // This is the case for the center point. First checking if it's currently a
 807 |   // maximum.
 808 |   if ((hough_space(kRadius, kAngle) > FLAGS_hough_1_threshold) &&
 809 |       isLocalMaxima(hough_space, kAngle, kRadius)) {
 810 |     bool add_maximum = true;
 811 |     // Check if it was a maximum previously.
 812 |     for (const auto &maximum : previous_maxima) {
 813 |       if ((maximum.polarity == polarity) && (kRadius == maximum.r) &&
 814 |           (kAngle == maximum.theta_idx)) {
 815 |         add_maximum = false;
 816 |         break;
 817 |       }
 818 |     }
 819 | 
 820 |     // If required, add it to the list.
 821 |     if (add_maximum) {
 822 |       new_maxima.push_back(addMaxima(kAngle, kRadius, kTimestamp, polarity));
 823 |       new_maxima_value.push_back(hough_space(kRadius, kAngle));
 824 |     }
 825 |   }
 826 |   return false;
 827 | }
 828 | 
 829 | // Computing a full Non-Maximum Suppression for a given current Hough Space.
 830 | void Detector::computeFullNMS(
 831 |     const int time_step, const int nms_recompute_window,
 832 |     const MatrixHough &total_hough_space_pos,
 833 |     const MatrixHough &total_hough_space_neg,
 834 |     std::vector<std::vector<hough2map::Detector::line>> &cur_maxima_list) {
 835 | 
 836 |   // Index of the current event in the frame of all events of the current
 837 |   // message with carry-over from previous message.
 838 |   const int kNmsIndex =
 839 |       FLAGS_hough_1_window_size + time_step * nms_recompute_window;
 840 |   std::vector<hough2map::Detector::line> &current_maxima =
 841 |       cur_maxima_list[kNmsIndex];
 842 | 
 843 |   // New detected maxima and their value.
 844 |   std::vector<hough2map::Detector::line> new_maxima;
 845 |   std::vector<int> new_maxima_value;
 846 | 
 847 |   // Checking every angle and radius hypothesis.
 848 |   for (int i = 0; i < kHough1AngularResolution; i++) {
 849 |     for (int j = 0; j < kHough1RadiusResolution; j++) {
 850 |       // Get the current events for a current time stamp.
 851 |       const dvs_msgs::Event &event = feature_msg_.events[kNmsIndex];
 852 | 
 853 |       // Checking positive Hough space, whether it is larger than threshold
 854 |       // and larger than neighbors.
 855 |       if (total_hough_space_pos(j, i) > FLAGS_hough_1_threshold) {
 856 |         if (isLocalMaxima(total_hough_space_pos, i, j)) {
 857 |           // Add as a possible maximum to the list.
 858 |           new_maxima.push_back(addMaxima(i, j, event.ts.toSec(), true));
 859 |           new_maxima_value.push_back(total_hough_space_pos(j, i));
 860 |         }
 861 |       }
 862 | 
 863 |       // Checking positive Hough space, whether it is larger than threshold
 864 |       // and larger than neighbors.
 865 |       if (total_hough_space_neg(j, i) > FLAGS_hough_1_threshold) {
 866 |         if (isLocalMaxima(total_hough_space_neg, i, j)) {
 867 |           // Add as a possible maximum to the list.
 868 |           new_maxima.push_back(addMaxima(i, j, event.ts.toSec(), false));
 869 |           new_maxima_value.push_back(total_hough_space_neg(j, i));
 870 |         }
 871 |       }
 872 |     }
 873 |   }
 874 |   // This applies a suppression radius to the list of maxima hypothesis, to
 875 |   // only get the most prominent ones.
 876 |   applySuppressionRadius(new_maxima, new_maxima_value, &current_maxima);
 877 | }
 878 | 
 879 | // Computing a full Hough Space based on a current set of events and their
 880 | // respective Hough parameters.
 881 | void Detector::computeFullHoughTransform(const int time_step,
 882 |                                          const int nms_recompute_window,
 883 |                                          MatrixHough &total_hough_space_pos,
 884 |                                          MatrixHough &total_hough_space_neg,
 885 |                                          const Eigen::MatrixXi &radii) {
 886 |   // Looping over all events that have an influence on the current total
 887 |   // Hough space, so the past 300.
 888 |   const int kRight =
 889 |       FLAGS_hough_1_window_size + time_step * nms_recompute_window;
 890 |   const int kLeft = kRight - FLAGS_hough_1_window_size;
 891 |   CHECK_GT(kRight, kLeft);
 892 | 
 893 |   for (int j = kRight; j > kLeft; j--) {
 894 |     // Looping over all confirmed hypothesis and adding them to the Hough
 895 |     // space.
 896 |     updateHoughSpaceVotes(true, j, feature_msg_.events[j].polarity, radii,
 897 |                           total_hough_space_pos, total_hough_space_neg);
 898 |   }
 899 | }
 900 | 
 901 | // Incrementing a HoughSpace for a certain event.
 902 | void Detector::updateHoughSpaceVotes(const bool increment, const int event_idx,
 903 |                                      const bool pol,
 904 |                                      const Eigen::MatrixXi &radii,
 905 |                                      MatrixHough &hough_space_pos,
 906 |                                      MatrixHough &hough_space_neg) {
 907 |   // Looping over all confirmed hypothesis and adding or removing them from the
 908 |   // Hough space.
 909 |   for (int k = 0; k < kHough1AngularResolution; k++) {
 910 |     const int kRadius = radii(k, event_idx);
 911 |     // making sure the parameter set is within the domain of the HS.
 912 |     if ((kRadius >= 0) && (kRadius < kHough1RadiusResolution)) {
 913 |       // Incrementing or decrement the respective accumulator cells.
 914 |       if (pol) {
 915 |         if (increment) {
 916 |           hough_space_pos(kRadius, k)++;
 917 |         } else {
 918 |           hough_space_pos(kRadius, k)--;
 919 |         }
 920 |       } else {
 921 |         if (increment) {
 922 |           hough_space_neg(kRadius, k)++;
 923 |         } else {
 924 |           hough_space_neg(kRadius, k)--;
 925 |         }
 926 |       }
 927 |     }
 928 |   }
 929 | }
 930 | 
 931 | // Apply the scond Hough tranform.
 932 | void Detector::secondHoughTransform(
 933 |     const std::vector<std::vector<hough2map::Detector::line>>
 934 |         &cur_maxima_list) {
 935 | 
 936 |   int num_events = feature_msg_.events.size();
 937 | 
 938 |   // Second Hough tranform. x-t-space
 939 |   // Track the obtained maxima over time.
 940 | 
 941 |   // Step 1: Reshape maxima list into x-t data.
 942 |   // Step 2: Discretizing maxima into timesteps.
 943 | 
 944 |   Eigen::Matrix<int, kHough1RadiusResolution, kHough2TimestepsPerMsg>
 945 |       tracked_maxima_pos;
 946 |   Eigen::Matrix<int, kHough1RadiusResolution, kHough2TimestepsPerMsg>
 947 |       tracked_maxima_neg;
 948 | 
 949 |   tracked_maxima_pos.setZero();
 950 |   tracked_maxima_neg.setZero();
 951 | 
 952 |   const double kTimestampMsgBegin = feature_msg_.events[0].ts.toSec();
 953 | 
 954 |   CHECK_GT(kHough2TimestepsPerMsg, 0);
 955 |   const double kColumnLengthInSec =
 956 |       (1.0 / FLAGS_event_array_frequency) / (kHough2TimestepsPerMsg);
 957 | 
 958 |   for (int i = 0; i < num_events; i++) {
 959 |     const dvs_msgs::Event &e = feature_msg_.events[i];
 960 |     for (auto &maxima : cur_maxima_list[i]) {
 961 |       // Not every message is exactly 33ms long. To deal with this, we currently
 962 |       // squeeze longer messages down to be 33ms long.
 963 | 
 964 |       const int time_idx =
 965 |           std::min((int)std::floor((e.ts.toSec() - kTimestampMsgBegin) /
 966 |                                    kColumnLengthInSec),
 967 |                    kHough2TimestepsPerMsg - 1);
 968 | 
 969 |       CHECK_LT(time_idx, kHough2TimestepsPerMsg)
 970 |           << ": Something wrong with the time index!" + time_idx;
 971 | 
 972 |       // This discretizes the maxima. Many will be same or super close, so we
 973 |       // accumulate them.
 974 |       if (maxima.polarity) {
 975 |         tracked_maxima_pos(maxima.r, time_idx)++;
 976 |       } else {
 977 |         tracked_maxima_neg(maxima.r, time_idx)++;
 978 |       }
 979 |     }
 980 |   }
 981 | 
 982 |   // Ok, all the maxima are nicely arranged. Now I can do another Hough
 983 |   // transform with them,but first I want to look at them I need to establish
 984 |   // a window of points.
 985 | 
 986 |   // All maxima are now arranged in the current time window and discretized to
 987 |   // reduce the required computation time. We can now apply the next Hough
 988 |   // Transform.
 989 | 
 990 |   // Size of window depending on number of messages in window, assuming 30
 991 |   // messages per second.
 992 | 
 993 |   const double kWindowSizeInSec =
 994 |       kHough2MsgPerWindow / FLAGS_event_array_frequency;
 995 | 
 996 |   const double kWindowEndTime = feature_msg_.events[num_events - 1].ts.toSec();
 997 | 
 998 |   // Window for data storage.
 999 |   hough2_queue_pos_.push_back(tracked_maxima_pos);
1000 |   hough2_queue_neg_.push_back(tracked_maxima_neg);
1001 |   CHECK_EQ(hough2_queue_pos_.size(), hough2_queue_neg_.size())
1002 |       << ": Something wrong with window size of the 2nd hough transform!";
1003 | 
1004 |   // Removing old stuff when the window is full.
1005 |   if (hough2_queue_pos_.size() > kHough2MsgPerWindow) {
1006 |     hough2_queue_pos_.pop_front();
1007 |     hough2_queue_neg_.pop_front();
1008 |   }
1009 | 
1010 |   // Initializing Hough spaces for the 2nd HT.
1011 |   const int test = kHough2AngularResolution;
1012 |   Eigen::MatrixXi hough_2_space_pos(
1013 |       kHough2MsgPerWindow * kHough2TimestepsPerMsg, test);
1014 |   Eigen::MatrixXi hough_2_space_neg(
1015 |       kHough2MsgPerWindow * kHough2TimestepsPerMsg, test);
1016 | 
1017 |   hough_2_space_pos.setZero();
1018 |   hough_2_space_neg.setZero();
1019 | 
1020 |   // For every message in the window.
1021 | 
1022 | #pragma omp parallel for
1023 |   for (int i = 0; i < hough2_queue_pos_.size(); i++) {
1024 |     for (int j = 0; j < kHough1RadiusResolution; j++) {
1025 |       for (int k = 0; k < kHough2TimestepsPerMsg; k++) {
1026 |         Eigen::Matrix<double, 2, 1> point;
1027 |         Eigen::Matrix<int, kHough2AngularResolution, 1> rho;
1028 | 
1029 |         point(0, 0) = i * kHough2TimestepsPerMsg + k;
1030 |         point(1, 0) = j;
1031 | 
1032 |         // Accumulated maxima from the discretization step now function as
1033 |         // weights for the HT.
1034 |         const int kHough2WeightsPos = hough2_queue_pos_[i](j, k);
1035 |         const int kHough2WeightsNeg = hough2_queue_neg_[i](j, k);
1036 | 
1037 |         rho = (polar_param_mapping_2_ * point).cast<int>();
1038 | 
1039 |         // For every angle.
1040 |         for (int l = 0; l < kHough2AngularResolution; l++) {
1041 |           // Do Hough transform.
1042 |           if ((rho(l, 0) > 0) &&
1043 |               (rho(l, 0) < kHough2MsgPerWindow * kHough2TimestepsPerMsg)) {
1044 |             hough_2_space_pos(rho(l, 0), l) += kHough2WeightsPos;
1045 |             hough_2_space_neg(rho(l, 0), l) += kHough2WeightsNeg;
1046 |           }
1047 |         }
1048 |       }
1049 |     }
1050 |   }
1051 | 
1052 |   std::vector<cv::Vec3f> detected_lines_pos;
1053 |   std::vector<cv::Vec3f> detected_lines_neg;
1054 | 
1055 |   // Tuning parameter for enforcing separation of maxima.
1056 |   const float kMinAngleSep = FLAGS_hough_2_nms_min_angle_separation *
1057 |                              FLAGS_hough_2_nms_min_angle_separation;
1058 |   const float kMinRadiusSep = FLAGS_hough_2_nms_min_rho_separation *
1059 |                               FLAGS_hough_2_nms_min_rho_separation;
1060 | 
1061 |   for (int i = 0; i < kHough2AngularResolution; i++) {
1062 |     for (int j = 0; j < kHough2MsgPerWindow * kHough2TimestepsPerMsg; j++) {
1063 |       // Checking positive Hough space.
1064 |       hough2nms(i, j, hough_2_space_pos, detected_lines_pos);
1065 | 
1066 |       // Checking negative Hough space.
1067 |       hough2nms(i, j, hough_2_space_neg, detected_lines_neg);
1068 |     }
1069 |   }
1070 | 
1071 |   // At this point we check whether positive and negative line detections line
1072 |   // up. If there is a pole infront of the camera, there will be a positive and
1073 |   // a negative detection in parallel an close proximity from the two
1074 |   // edges of the pole. If it is another object, such as a building or
1075 |   // bridge, these two edges will be separated much further.
1076 |   for (size_t i = 0; i < detected_lines_pos.size(); i++) {
1077 |     // Parameters of current positive line.
1078 |     const float kRhoPos = detected_lines_pos[i][0];
1079 |     const float kThetaPos = detected_lines_pos[i][1];
1080 |     // Compare against all current negative lines (typically there are only a
1081 |     // hand full of lines simultaneously, so not so expensive).
1082 |     for (size_t j = 0; j < detected_lines_neg.size(); j++) {
1083 |       const float kRhoNeg = detected_lines_neg[j][0];
1084 |       const float kThetaNeg = detected_lines_neg[j][1];
1085 | 
1086 |       const float kPosNegRadiusSeparation =
1087 |           (kRhoPos - kRhoNeg) * (kRhoPos - kRhoNeg);
1088 |       const float kPosNegAngularSeparation =
1089 |           (kThetaPos - kThetaNeg) * (kThetaPos - kThetaNeg);
1090 | 
1091 |       // If the two lines are nearly parallel and relativity close to each other
1092 |       // pixel wise, they are approved to be a pole detection.
1093 |       if ((kPosNegAngularSeparation <
1094 |            FLAGS_hough_2_nms_neg_pos_angular_matching) &&
1095 |           (kPosNegRadiusSeparation <
1096 |            FLAGS_hough_2_nms_neg_pos_radial_matching)) {
1097 |         // Compute pole timestamps. I need the pole speed in px/s and a
1098 |         // timestamp for finding the according train speed.
1099 | 
1100 |         double timestamp_enter;
1101 |         double timestamp_leave;
1102 |         const double kWindowTimestampBeginning =
1103 |             kWindowEndTime - kWindowSizeInSec;
1104 | 
1105 |         timestamp_leave = (1 / cos(kThetaPos)) * (kRhoPos);
1106 |         timestamp_enter = (1 / cos(kThetaPos)) *
1107 |                           (kRhoPos - kHough2AngularResolution * sin(kThetaPos));
1108 | 
1109 |         timestamp_enter =
1110 |             (timestamp_enter / (kHough2MsgPerWindow * kHough2TimestepsPerMsg)) *
1111 |                 kWindowSizeInSec +
1112 |             kWindowTimestampBeginning;
1113 |         timestamp_leave =
1114 |             (timestamp_leave / (kHough2MsgPerWindow * kHough2TimestepsPerMsg)) *
1115 |                 kWindowSizeInSec +
1116 |             kWindowTimestampBeginning;
1117 | 
1118 |         CHECK_GT(timestamp_leave, timestamp_enter)
1119 |             << ":Timestamps seem to be wrong, the pole leaves before it "
1120 |                "enters?!";
1121 | 
1122 |         // If an odometry is available, we can triangulate the new pole.
1123 |         if (FLAGS_odometry_available) {
1124 |           newPoleDetection(kRhoPos, kThetaPos, kWindowTimestampBeginning, true);
1125 |         }
1126 |       }
1127 |     }
1128 |   }
1129 | 
1130 |   // Plot the lines.
1131 |   if (FLAGS_display_2nd_hough_space) {
1132 |     visualizeSecondHoughSpace(detected_lines_pos, detected_lines_neg);
1133 |   }
1134 | }
1135 | 
1136 | // Second Hough space non maximum suppression.
1137 | void Detector::hough2nms(const int i, const int j,
1138 |                          const Eigen::MatrixXi &hough_2_space,
1139 |                          std::vector<cv::Vec3f> &detections) {
1140 |   // Tuning parameter for enforcing separation of maxima.
1141 |   const float kMinAngleSep = FLAGS_hough_2_nms_min_angle_separation *
1142 |                              FLAGS_hough_2_nms_min_angle_separation;
1143 |   const float kMinRadiusSep = FLAGS_hough_2_nms_min_rho_separation *
1144 |                               FLAGS_hough_2_nms_min_rho_separation;
1145 | 
1146 |   if (hough_2_space(j, i) > FLAGS_hough_2_min_threshold) {
1147 |     if (isLocalMaxima(hough_2_space, i, j)) {
1148 |       cv::Vec3f new_vector;
1149 |       new_vector[0] = j;
1150 |       new_vector[1] = thetas_2_(i);
1151 |       new_vector[2] = hough_2_space(j, i);
1152 | 
1153 |       // Check if lines already exist.
1154 |       if (detections.size() > 0) {
1155 | 
1156 |         bool add_to_list = true;
1157 |         for (size_t k = 0; k < detections.size(); k++) {
1158 |           const float kRho = detections[k][0];
1159 |           const float kTheta = detections[k][1];
1160 |           const float kVal = detections[k][2];
1161 | 
1162 |           const float kCurAngleSeparation =
1163 |               (kTheta - new_vector[1]) * (kTheta - new_vector[1]);
1164 |           const float kCurRadiusSeparation =
1165 |               (kRho - new_vector[0]) * (kRho - new_vector[0]);
1166 | 
1167 |           // If line is close, check which is larger.
1168 |           if ((kCurAngleSeparation < kMinAngleSep) &&
1169 |               (kCurRadiusSeparation < kMinRadiusSep)) {
1170 |             add_to_list = false;
1171 | 
1172 |             // overwrite or discard
1173 |             if (new_vector[2] > kVal) {
1174 |               detections[k] = new_vector;
1175 |             }
1176 |           }
1177 |         }
1178 | 
1179 |         // If none was bigger, add to the end.
1180 |         if (add_to_list) {
1181 |           detections.push_back(new_vector);
1182 |         }
1183 |       } else {
1184 |         // First line, so add it.
1185 |         detections.push_back(new_vector);
1186 |       }
1187 |     }
1188 |   }
1189 | }
1190 | 
1191 | // Event preprocessing prior to first HT.
1192 | void Detector::eventPreProcessing(
1193 |     const dvs_msgs::EventArray::ConstPtr &orig_msg, Eigen::MatrixXf &points) {
1194 | 
1195 |   int num_events = orig_msg->events.size();
1196 |   // Filtering for dead pixels and subsampling the leftover events.
1197 |   //
1198 |   // TODO: Could be parallelized by splitting into two steps, one that counts
1199 |   // second one that does the actual shuffle in memory, if done correctly
1200 |   // with some caching overhead would be very small.
1201 |   //
1202 |   // Also would be faster to just preallocate the eigen matrix to max
1203 |   // size and write directly into it and afterwards resize.
1204 |   for (int i = 0; i < num_events; i += FLAGS_event_subsample_factor) {
1205 |     const dvs_msgs::Event &e = orig_msg->events[i];
1206 | 
1207 |     // Seemingly broken pixels in the DVS (millions of exactly equal events at
1208 |     // once at random). This needs to be adapted if you use another device.
1209 |     if (((e.x != 19) || (e.y != 18)) && ((e.x != 43) || (e.y != 72)) &&
1210 |         ((e.x != 89) || (e.y != 52)) && ((e.x != 25) || (e.y != 42)) &&
1211 |         ((e.x != 61) || (e.y != 71)) && ((e.x != 37) || (e.y != 112))) {
1212 |       feature_msg_.events.push_back(e);
1213 |     }
1214 |   }
1215 | 
1216 |   // Number of events after filtering and subsampling.
1217 |   num_events = feature_msg_.events.size();
1218 | 
1219 |   // Check there are enough events for our window size. This is only relevant
1220 |   // during initialization.
1221 |   if (num_events <= FLAGS_hough_1_window_size) {
1222 |     return;
1223 |   }
1224 | 
1225 |   // Reshaping the event array into an Eigen matrix.
1226 |   points.resize(2, num_events);
1227 |   points.setZero();
1228 | 
1229 |   // Add points from the actual message.
1230 |   const auto ptr = points.data();
1231 |   CHECK_NOTNULL(ptr);
1232 | 
1233 |   if (FLAGS_perform_camera_undistortion) {
1234 | #pragma omp parallel for
1235 |     for (int i = 0; i < num_events; i++) {
1236 |       const dvs_msgs::Event &event = feature_msg_.events[i];
1237 | 
1238 |       *(ptr + 2 * i) = undist_map_x_(event.y, event.x);
1239 |       *(ptr + 2 * i + 1) = undist_map_y_(event.y, event.x);
1240 |     }
1241 |   } else {
1242 | #pragma omp parallel for
1243 |     for (int i = 0; i < num_events; i++) {
1244 |       const dvs_msgs::Event &event = feature_msg_.events[i];
1245 | 
1246 |       *(ptr + 2 * i) = event.x;
1247 |       *(ptr + 2 * i + 1) = event.y;
1248 |     }
1249 |   }
1250 | }
1251 | 
1252 | // Function for visualizing the current second Hough Space.
1253 | void Detector::visualizeSecondHoughSpace(
1254 |     const std::vector<cv::Vec3f> &kDetectionsPos,
1255 |     const std::vector<cv::Vec3f> &kDetectionsNeg) {
1256 | 
1257 |   // Window for visualization.
1258 |   cv::Mat line_space_pos(kHough1RadiusResolution,
1259 |                          kHough2MsgPerWindow * kHough2TimestepsPerMsg, CV_8UC1,
1260 |                          1);
1261 |   cv::Mat line_space_neg(kHough1RadiusResolution,
1262 |                          kHough2MsgPerWindow * kHough2TimestepsPerMsg, CV_8UC1,
1263 |                          1);
1264 | 
1265 | #pragma omp parallel for
1266 |   for (int i = 0; i < hough2_queue_pos_.size(); i++) {
1267 |     for (int j = 0; j < hough2_queue_pos_[i].rows(); j++) {
1268 |       for (int k = 0; k < kHough2TimestepsPerMsg; k++) {
1269 |         line_space_pos.at<uchar>(j, i * kHough2TimestepsPerMsg + k, 0) =
1270 |             hough2_queue_pos_[i](j, k);
1271 |       }
1272 |     }
1273 |     for (int j = 0; j < hough2_queue_neg_[i].rows(); j++) {
1274 |       for (int k = 0; k < kHough2TimestepsPerMsg; k++) {
1275 |         line_space_neg.at<uchar>(j, i * kHough2TimestepsPerMsg + k, 0) =
1276 |             hough2_queue_neg_[i](j, k);
1277 |       }
1278 |     }
1279 |   }
1280 | 
1281 |   cv::cvtColor(line_space_pos, line_space_pos, cv::COLOR_GRAY2BGR);
1282 |   cv::cvtColor(line_space_neg, line_space_neg, cv::COLOR_GRAY2BGR);
1283 | 
1284 |   for (size_t i = 0; i < kDetectionsPos.size(); i++) {
1285 |     float rho = kDetectionsPos[i][0], theta = kDetectionsPos[i][1];
1286 |     drawPolarCorLine(line_space_pos, rho, theta, cv::Scalar(255, 0, 0));
1287 |   }
1288 | 
1289 |   for (size_t i = 0; i < kDetectionsNeg.size(); i++) {
1290 |     float rho = kDetectionsNeg[i][0], theta = kDetectionsNeg[i][1];
1291 |     drawPolarCorLine(line_space_neg, rho, theta, cv::Scalar(0, 0, 255));
1292 |   }
1293 | 
1294 |   // Flip image for nicer viewing.
1295 |   cv::Mat out;
1296 | 
1297 |   cv::flip(line_space_pos, line_space_pos, 0);
1298 |   cv::flip(line_space_neg, line_space_neg, 0);
1299 | 
1300 |   cv::vconcat(line_space_pos, line_space_neg, out);
1301 | 
1302 |   cv::imshow("Hough Transform #2", out);
1303 |   cv::waitKey(1);
1304 | }
1305 | 
1306 | void Detector::drawPolarCorLine(cv::Mat &image_space, float rho, float theta,
1307 |                                 cv::Scalar color) {
1308 |   // Function to draw a line based on polar coordinates
1309 |   cv::Point pt1, pt2;
1310 |   const double a = cos(theta), b = sin(theta);
1311 |   const double x0 = a * rho, y0 = b * rho;
1312 |   pt1.x = cvRound(x0 + 1000 * (-b));
1313 |   pt1.y = cvRound(y0 + 1000 * (a));
1314 |   pt2.x = cvRound(x0 - 1000 * (-b));
1315 |   pt2.y = cvRound(y0 - 1000 * (a));
1316 |   cv::line(image_space, pt1, pt2, color, 3, cv::LINE_AA);
1317 | }
1318 | 
1319 | void Detector::addMaximaInRadius(
1320 |     int i, int radius, const MatrixHough &total_hough_space,
1321 |     int local_threshold, bool polarity, double timestamp,
1322 |     std::vector<hough2map::Detector::line> *new_maxima,
1323 |     std::vector<int> *new_maxima_value, bool skip_center) {
1324 |   int m_l = std::max(i - FLAGS_hough_space_NMS_suppression_radius, 0);
1325 |   int m_r = std::min(i + FLAGS_hough_space_NMS_suppression_radius + 1,
1326 |                      kHough1AngularResolution);
1327 |   int n_l = std::max(radius - FLAGS_hough_space_NMS_suppression_radius, 0);
1328 |   int n_r = std::min(radius + FLAGS_hough_space_NMS_suppression_radius + 1,
1329 |                      kHough1RadiusResolution);
1330 | 
1331 |   for (int m = m_l; m < m_r; m++) {
1332 |     for (int n = n_l; n < n_r; n++) {
1333 |       if (skip_center && (n == radius) && (m == i)) {
1334 |         continue;
1335 |       }
1336 | 
1337 |       if ((total_hough_space(n, m) > local_threshold) &&
1338 |           isLocalMaxima(total_hough_space, m, n)) {
1339 |         new_maxima->push_back(addMaxima(m, n, timestamp, polarity));
1340 |         new_maxima_value->push_back(total_hough_space(n, m));
1341 |       }
1342 |     }
1343 |   }
1344 | }
1345 | 
1346 | void Detector::applySuppressionRadius(
1347 |     const std::vector<hough2map::Detector::line> &new_maxima,
1348 |     const std::vector<int> &new_maxima_value,
1349 |     std::vector<hough2map::Detector::line> *current_maxima) {
1350 | 
1351 |   // Create an index of all known maxima.
1352 |   std::vector<int> new_maxima_index(new_maxima_value.size());
1353 |   for (int i = 0; i < new_maxima_value.size(); i++) {
1354 |     new_maxima_index[i] = i;
1355 |   }
1356 | 
1357 |   // Sort the index of all currently known maxima. Sort them by: 1. value; 2.
1358 |   // rho value; 3. theta value.
1359 |   std::stable_sort(
1360 |       new_maxima_index.begin(), new_maxima_index.end(),
1361 |       [&new_maxima_value, &new_maxima](const int i1, const int i2) {
1362 |         if (new_maxima_value[i1] != new_maxima_value[i2]) {
1363 |           return new_maxima_value[i1] > new_maxima_value[i2];
1364 |         } else {
1365 |           if (new_maxima[i1].r != new_maxima[i2].r) {
1366 |             return new_maxima[i1].r > new_maxima[i2].r;
1367 |           } else {
1368 |             return new_maxima[i1].theta_idx > new_maxima[i2].theta_idx;
1369 |           }
1370 |         }
1371 |       });
1372 | 
1373 |   // Clear buffer of current maxima to re-add them later on.
1374 |   current_maxima->clear();
1375 | 
1376 |   // Loop over all maxima according to the sorted order.
1377 |   for (int i = 0; i < new_maxima.size(); i++) {
1378 |     const hough2map::Detector::line &new_maximum =
1379 |         new_maxima[new_maxima_index[i]];
1380 | 
1381 |     bool add_maximum = true;
1382 |     // Compare to all other maxima in the output buffer.
1383 |     for (int j = 0; j < current_maxima->size(); j++) {
1384 |       const hough2map::Detector::line &cur_maximum = (*current_maxima)[j];
1385 | 
1386 |       // If no maximum in the output buffer is of the same polarity and within
1387 |       // the radius, the current maximum is kept and added to the output
1388 |       // buffer.
1389 |       if (cur_maximum.polarity == new_maximum.polarity) {
1390 |         // Suppression radius.
1391 |         float distance =
1392 |             (cur_maximum.r - new_maximum.r) * (cur_maximum.r - new_maximum.r) +
1393 |             (cur_maximum.theta_idx - new_maximum.theta_idx) *
1394 |                 (cur_maximum.theta_idx - new_maximum.theta_idx);
1395 | 
1396 |         if (distance < FLAGS_hough_space_NMS_suppression_radius *
1397 |                            FLAGS_hough_space_NMS_suppression_radius) {
1398 |           add_maximum = false;
1399 |           break;
1400 |         }
1401 |       }
1402 |     }
1403 | 
1404 |     // Adding accepted maxima to the output buffer.
1405 |     if (add_maximum) {
1406 |       current_maxima->push_back(new_maximum);
1407 |     }
1408 |   }
1409 | }
1410 | 
1411 | // Check if the center value is a maxima.
1412 | bool Detector::isLocalMaxima(const Eigen::MatrixXi &hough_space, int i,
1413 |                              int radius) {
1414 |   // Define the 8-connected neighborhood.
1415 |   const int m_l = std::max(i - 1, 0);
1416 |   const int m_r = std::min(i + 1, (int)hough_space.cols() - 1);
1417 | 
1418 |   const int n_l = std::max(radius - 1, 0);
1419 |   const int n_r = std::min(radius + 1, (int)hough_space.rows() - 1);
1420 | 
1421 |   // Loop over all neighborhood points
1422 |   for (int m = m_l; m <= m_r; m++) {
1423 |     for (int n = n_l; n <= n_r; n++) {
1424 |       if ((m != i) || (n != radius)) {
1425 |         if (hough_space(n, m) >= hough_space(radius, i)) {
1426 |           // If anyone was larger or equal, it's not a maximum.
1427 |           return false;
1428 |         }
1429 |       }
1430 |     }
1431 |   }
1432 | 
1433 |   return true;
1434 | }
1435 | 
1436 | // Callback function for saving current greyscale image frame.
1437 | void Detector::imageCallback(const sensor_msgs::Image::ConstPtr &msg) {
1438 |   // Get greyscale image.
1439 |   cv::Mat cv_image_raw;
1440 |   cv_bridge::CvImagePtr cv_ptr;
1441 | 
1442 |   try {
1443 |     cv_ptr = cv_bridge::toCvCopy(msg);
1444 |   } catch (cv_bridge::Exception &e) {
1445 |     ROS_ERROR("cv_bridge exception: %s", e.what());
1446 |     return;
1447 |   }
1448 | 
1449 |   cur_greyscale_img_ = cv_ptr->image;
1450 |   cv::cvtColor(cur_greyscale_img_, cur_greyscale_img_, cv::COLOR_GRAY2BGR);
1451 | }
1452 | 
1453 | void Detector::newPoleDetection(double rho, double theta, double window_time,
1454 |                                 bool pol) {
1455 |   // Creating new pole object. This is on the one hand legacy code, on the
1456 |   // other hand ready for future on the go map storage, or something like
1457 |   // that.
1458 |   pole new_pole;
1459 | 
1460 |   new_pole.rho = rho;
1461 |   new_pole.theta = theta;
1462 |   new_pole.polarity = pol;
1463 | 
1464 |   // Find the point in time of the first observation.
1465 |   double y = camera_resolution_width_;
1466 |   double first_observation = (1 / cos(theta)) * (rho - y * sin(theta));
1467 |   const double kWindowSizeInSec =
1468 |       kHough2MsgPerWindow / FLAGS_event_array_frequency;
1469 | 
1470 |   // Convert window time-steps to continous time.
1471 |   first_observation *=
1472 |       kWindowSizeInSec / (kHough2MsgPerWindow * kHough2TimestepsPerMsg);
1473 |   first_observation += window_time;
1474 | 
1475 |   new_pole.first_observed = first_observation;
1476 | 
1477 |   // The observation timestamp of the pole is still within the timespan
1478 |   // covered by the odometry buffer.
1479 |   if (first_observation > raw_gps_buffer_.front()[0]) {
1480 | 
1481 |     // Query raw gps position at observation beginning time.
1482 |     Eigen::Vector3d last_position =
1483 |         queryOdometryBuffer(first_observation, raw_gps_buffer_);
1484 |     Eigen::Vector3d last_velocity =
1485 |         queryOdometryBuffer(first_observation, velocity_buffer_);
1486 | 
1487 |     // In the next step we want to inspect the pole at each observation
1488 |     // timepoint, so each time it moved from one pixel to the next. This means
1489 |     // that we turn the continous spatio-temporal line of the pole observation
1490 |     // back into individual x-t points, by sampling the line at every possible x
1491 |     // (every horizontal pixel).
1492 | 
1493 |     std::vector<Eigen::Vector2d> pixel_pos;
1494 |     std::vector<Eigen::Matrix<double, 2, 3>> projection_mats;
1495 |     std::vector<Eigen::Matrix3d> transformation_mats;
1496 | 
1497 |     // Observation at each horizontal pixel position.
1498 |     for (int i = camera_resolution_width_; i > 0; i--) {
1499 |       // Timestamp of this observation.
1500 |       double observation_timestamp = (1 / cos(theta)) * (rho - i * sin(theta));
1501 |       observation_timestamp *=
1502 |           kWindowSizeInSec / (kHough2MsgPerWindow * kHough2TimestepsPerMsg);
1503 |       observation_timestamp += window_time;
1504 | 
1505 |       CHECK_GE(observation_timestamp, first_observation)
1506 |           << ":Something is wrong with observation odometry integration!";
1507 | 
1508 |       // Integrate odometry to get respective train transformations.
1509 |       if (observation_timestamp <= raw_gps_buffer_.back()[0]) {
1510 |         // Get the latest odometry.
1511 |         Eigen::Vector3d cur_velocity =
1512 |             queryOdometryBuffer(observation_timestamp, velocity_buffer_);
1513 |         Eigen::Vector2d cur_orientation =
1514 |             queryOdometryBuffer(observation_timestamp, orientation_buffer_);
1515 | 
1516 |         Eigen::Vector3d new_position = last_position;
1517 | 
1518 |         // Compute deltaT.
1519 |         const double diff = observation_timestamp - last_position[0];
1520 | 
1521 |         // Integrating odom.
1522 |         new_position[0] = observation_timestamp;
1523 |         new_position[1] += 0.5 * (cur_velocity[1] + last_velocity[1]) * diff;
1524 |         new_position[2] += 0.5 * (cur_velocity[2] + last_velocity[2]) * diff;
1525 | 
1526 |         last_velocity = cur_velocity;
1527 |         last_position = new_position;
1528 | 
1529 |         // Assemble train transformation matrix.
1530 |         Eigen::Matrix3d train_to_world_transformation;
1531 | 
1532 |         train_to_world_transformation << std::cos(cur_orientation[1]),
1533 |             std::sin(cur_orientation[1]), new_position[1],
1534 |             -std::sin(cur_orientation[1]), std::cos(cur_orientation[1]),
1535 |             new_position[2], 0, 0, 1;
1536 | 
1537 |         // Compute pole position in global frame.
1538 |         Eigen::Matrix3d T_camera_to_world;
1539 |         T_camera_to_world = (train_to_world_transformation * C_camera_train_ *
1540 |                              camera_train_offset_ * gps_offset_);
1541 | 
1542 |         Eigen::Matrix3d T_gps_to_world;
1543 |         T_gps_to_world =
1544 |             (train_to_world_transformation * C_camera_train_ * gps_offset_);
1545 | 
1546 |         // Invert train transformation matrix to get world to camera
1547 |         // transformaiton.
1548 |         Eigen::Matrix2d rot_part = T_camera_to_world.block<2, 2>(0, 0);
1549 |         Eigen::Vector2d trans_part = T_camera_to_world.block<2, 1>(0, 2);
1550 | 
1551 |         Eigen::Matrix3d world_to_camera_transformation;
1552 |         world_to_camera_transformation = Eigen::Matrix3d::Identity();
1553 |         world_to_camera_transformation.block<2, 2>(0, 0) = rot_part.transpose();
1554 |         world_to_camera_transformation.block<2, 1>(0, 2) =
1555 |             -rot_part.transpose() * trans_part;
1556 | 
1557 |         // Formulate projection matrix.
1558 |         Eigen::Matrix<double, 2, 3> cam_matrix;
1559 |         cam_matrix << intrinsics_[0], 0, intrinsics_[2], 0, 0, 1;
1560 | 
1561 |         Eigen::Matrix<double, 2, 3> projection_matrix;
1562 |         projection_matrix = cam_matrix * world_to_camera_transformation;
1563 | 
1564 |         // Everything I need for a DLT trianguaiton.
1565 |         Eigen::Vector2d pixel_position(i, 1);
1566 |         pixel_pos.push_back(pixel_position);
1567 |         projection_mats.push_back(projection_matrix);
1568 |         transformation_mats.push_back(world_to_camera_transformation);
1569 |       }
1570 |     }
1571 | 
1572 |     // Use a Singular Value Decomposition (SVD) to perform the triangulation.
1573 |     // This is also known as a Direct Linear Transform (DLT).
1574 |     int num_rows = projection_mats.size();
1575 | 
1576 |     // At least two observations are required for a triangulation.
1577 |     if (num_rows > 2) {
1578 |       // Assemble matrix A for DLT.
1579 |       Eigen::MatrixXd A;
1580 |       A.resize(num_rows, 3);
1581 |       for (int i = 0; i < projection_mats.size(); i++) {
1582 |         // Convert pixel frame to cam frame.
1583 |         double position = ((pixel_pos[i][0] - intrinsics_[3]) / intrinsics_[0]);
1584 | 
1585 |         A.row(i) = position * transformation_mats[i].row(1) -
1586 |                    transformation_mats[i].row(0);
1587 |       }
1588 |       // Singular Value decomposition.
1589 |       Eigen::BDCSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU |
1590 |                                                 Eigen::ComputeThinV);
1591 | 
1592 |       // Get the last column.
1593 |       Eigen::Vector3d x = svd.matrixV().col(svd.matrixV().cols() - 1);
1594 | 
1595 |       // Normalize homogenous coordinates.
1596 |       new_pole.pos_x = x[0] / x[2];
1597 |       new_pole.pos_y = x[1] / x[2];
1598 | 
1599 |       // Store new map point in file.
1600 |       if (FLAGS_map_output) {
1601 | 
1602 |         map_file << std::fixed << "0"
1603 |                  << ","
1604 |                  << "pole"
1605 |                  << "," << new_pole.first_observed << "," << new_pole.pos_x
1606 |                  << "," << new_pole.pos_y << ","
1607 |                  << "0"
1608 |                  << ","
1609 |                  << "0" << std::endl;
1610 |       }
1611 |     }
1612 |   }
1613 | }
1614 | 
1615 | // Just a funciton for creating new line structs.
1616 | Detector::line Detector::addMaxima(int angle, int rad, double cur_time,
1617 |                                    bool pol) {
1618 |   hough2map::Detector::line new_line;
1619 | 
1620 |   new_line.ID = 0;
1621 |   new_line.r = rad;
1622 |   new_line.theta = thetas_1_(angle);
1623 |   new_line.theta_idx = angle;
1624 |   new_line.time = cur_time;
1625 |   new_line.polarity = pol;
1626 | 
1627 |   return new_line;
1628 | }
1629 | 
1630 | // Generalized buffer query function for all odometry buffers.
1631 | template <class S, int rows, int cols>
1632 | Eigen::Matrix<S, rows, cols> Detector::queryOdometryBuffer(
1633 |     const double query_time,
1634 |     const std::deque<Eigen::Matrix<S, rows, cols>> &odometry_buffer) {
1635 |   // Getting data for current time from respective odometry buffer.
1636 | 
1637 |   // Check that required timestamp is within buffer.
1638 |   CHECK_GT(query_time, odometry_buffer[0][0])
1639 |       << ": Query time out of range of buffer!";
1640 | 
1641 |   if (query_time >= odometry_buffer[odometry_buffer.size() - 1][0]) {
1642 |     return odometry_buffer[odometry_buffer.size() - 1];
1643 |   }
1644 | 
1645 |   // Finding the the upper closest and lower closest data points for
1646 |   // interpolation.
1647 |   auto lower_it =
1648 |       std::upper_bound(
1649 |           odometry_buffer.begin(), odometry_buffer.end(), query_time,
1650 |           [](double lhs, Eigen::Matrix<S, rows, cols> rhs) -> bool {
1651 |             return lhs < rhs[0];
1652 |           }) -
1653 |       1;
1654 | 
1655 |   auto upper_it = std::lower_bound(
1656 |       odometry_buffer.begin(), odometry_buffer.end(), query_time,
1657 |       [](Eigen::Matrix<S, rows, cols> lhs, double rhs) -> bool {
1658 |         return lhs[0] < rhs;
1659 |       });
1660 | 
1661 |   // Interpolate datapoint.
1662 |   double delta =
1663 |       (query_time - (*lower_it)[0]) / ((*upper_it)[0] - (*lower_it)[0]);
1664 | 
1665 |   Eigen::Matrix<S, rows, cols> interpolation_result =
1666 |       (*lower_it) + delta * ((*upper_it) - (*lower_it));
1667 | 
1668 |   return interpolation_result;
1669 | }
1670 | 
1671 | // Function for converting lat/lon to UTM messages.
1672 | Detector::utm_coordinate Detector::deg2utm(double la, double lo) {
1673 |   utm_coordinate result;
1674 | 
1675 |   constexpr double sa = 6378137.000000;
1676 |   constexpr double sb = 6356752.314245;
1677 | 
1678 |   constexpr double e2 = std::sqrt((sa * sa) - (sb * sb)) / sb;
1679 | 
1680 |   constexpr double e2squared = e2 * e2;
1681 | 
1682 |   constexpr double c = (sa * sa) / sb;
1683 | 
1684 |   const double lat = la * M_PI / 180;
1685 |   const double lon = lo * M_PI / 180;
1686 | 
1687 |   const double H_d = (lo / 6) + 31;
1688 |   int H;
1689 | 
1690 |   if (H_d > 0) {
1691 |     H = std::floor(H_d);
1692 |   } else {
1693 |     H = std::ceil(H_d);
1694 |   }
1695 | 
1696 |   const int S = (H * 6) - 183;
1697 |   const double deltaS = lon - (S * (M_PI / 180));
1698 | 
1699 |   char letter;
1700 | 
1701 |   if (la < -72)
1702 |     letter = 'C';
1703 |   else if (la < -64)
1704 |     letter = 'D';
1705 |   else if (la < -56)
1706 |     letter = 'E';
1707 |   else if (la < -48)
1708 |     letter = 'F';
1709 |   else if (la < -40)
1710 |     letter = 'G';
1711 |   else if (la < -32)
1712 |     letter = 'H';
1713 |   else if (la < -24)
1714 |     letter = 'J';
1715 |   else if (la < -16)
1716 |     letter = 'K';
1717 |   else if (la < -8)
1718 |     letter = 'L';
1719 |   else if (la < -0)
1720 |     letter = 'M';
1721 |   else if (la < 8)
1722 |     letter = 'N';
1723 |   else if (la < 16)
1724 |     letter = 'P';
1725 |   else if (la < 24)
1726 |     letter = 'Q';
1727 |   else if (la < 32)
1728 |     letter = 'R';
1729 |   else if (la < 40)
1730 |     letter = 'S';
1731 |   else if (la < 48)
1732 |     letter = 'T';
1733 |   else if (la < 56)
1734 |     letter = 'U';
1735 |   else if (la < 64)
1736 |     letter = 'V';
1737 |   else if (la < 72)
1738 |     letter = 'W';
1739 |   else
1740 |     letter = 'X';
1741 | 
1742 |   const double a = std::cos(lat) * std::sin(deltaS);
1743 |   const double epsilon = 0.5 * std::log((1 + a) / (1 - a));
1744 |   const double nu = std::atan(std::tan(lat) / std::cos(deltaS)) - lat;
1745 |   const double v =
1746 |       (c / std::sqrt((1 + (e2squared * (std::cos(lat) * std::cos(lat)))))) *
1747 |       0.9996;
1748 |   const double ta =
1749 |       (e2squared / 2) * epsilon * epsilon * (std::cos(lat) * std::cos(lat));
1750 |   const double a1 = std::sin(2 * lat);
1751 |   const double a2 = a1 * (std::cos(lat) * std::cos(lat));
1752 |   const double j2 = lat + (a1 / 2);
1753 |   const double j4 = ((3 * j2) + a2) / 4;
1754 |   const double j6 = ((5 * j4) + (a2 * (std::cos(lat) * std::cos(lat)))) / 3;
1755 | 
1756 |   const double alfa = (3.0 / 4) * e2squared;
1757 |   const double beta = (5.0 / 3) * alfa * alfa;
1758 |   const double gama = (35.0 / 27) * alfa * alfa * alfa;
1759 | 
1760 |   const double Bm = 0.9996 * c * (lat - alfa * j2 + beta * j4 - gama * j6);
1761 | 
1762 |   const double xx = epsilon * v * (1 + (ta / 3)) + 500000;
1763 |   double yy = nu * v * (1 + ta) + Bm;
1764 | 
1765 |   if (yy < 0) {
1766 |     yy = 9999999 + yy;
1767 |   }
1768 | 
1769 |   result.x = xx;
1770 |   result.y = yy;
1771 |   result.zone = std::to_string(H) + letter;
1772 | 
1773 |   return result;
1774 | }
1775 | 
1776 | } // namespace hough2map
1777 | 


--------------------------------------------------------------------------------