diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24cc12db0fe8cc6fd480995877afc40ecf7b4118..895a37d7b144faa8266e10e54f157cd2b038a5d8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,25 +11,25 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
endif()
#Set compiler according C++11 support
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
+#include(CheckCXXCompilerFlag)
+#CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+#CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+#if(COMPILER_SUPPORTS_CXX11)
+# message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+#elseif(COMPILER_SUPPORTS_CXX0X)
+# message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+#else()
+# message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+#endif()
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm message_generation cv_bridge)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -88,7 +88,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm std_msgs message_runtime
+ CATKIN_DEPENDS iri_base_algorithm std_msgs message_runtime
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
diff --git a/cfg/GroundSegmentation.cfg b/cfg/GroundSegmentation.cfg
index 54fa04ee05b9ab35998e02ae792bc5c97828341b..6505d99ad9a14d68729b4a94ddec1e317fb56a22 100644
--- a/cfg/GroundSegmentation.cfg
+++ b/cfg/GroundSegmentation.cfg
@@ -46,8 +46,9 @@ gen.add("robot_height", double_t, 0, "All obstacle points above this value (w.r.
# Parameters affecting the pointcloud exploration
gen.add("ROI_delta_x_and_y", double_t, 0, "ROI area = ( 2 * ROI_delta_x_and_y ) ^ 2", 2.0, 0.0, 10.0);
+gen.add("ROI_shadow_area", double_t, 0, "ROI for root vertex in shadow area = ( 2 * ROI_delta_x_and_y ) ^ 2", 6.0, 0.0, 10.0);
gen.add("ground_reference_search_resolution_deg", double_t, 0, "It is the angular resolution when generating new ground references (graph nodes) in degrees", 20.0, 1.0, 100.0);
-gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the elevation points", 0.5, 0.01, 10.0);
+gen.add("elevation_grid_resolution", double_t, 0, "define the coarseness of the elevation points", 0.5, 0.2, 10.0);
# Kalman filter noise parameters
## initial uncertainties
@@ -62,11 +63,20 @@ gen.add("z_observation_std_dev", double_t, 0, "Measurement std_dev", 0.2, 0.0, 1
# threshold for outlier rejection and classification
gen.add("mahalanobis_threshold", double_t, 0, "Max mahalanobis distance for ground points", 3.0, 0.01, 6.0);
+# Neural Network related parameters
+gen.add("use_neural_network", bool_t, 0, "uses neural network to segmentate ground points attending to intensity related features", False);
+gen.add("extract_data_to_external_training_of_the_network", bool_t, 0, "Extract features to external file to train with Matlab", False);
+gen.add("dataset_filename_with_global_path", str_t, 0, "name of the file where dump the features for neural net training in matlab", "");
+gen.add("neural_net_filename_with_global_path", str_t, 0, "name of the file containing the neural net weights", "");
+gen.add("neural_net_number_of_features", int_t, 0, "Size of the input layer", 9, 1, 100);
+gen.add("neural_net_number_of_neurons_in_hidden_layer", int_t, 0, "Size of the hidden layer", 25, 1, 100);
+gen.add("neural_net_number_of_neurons_in_output_layer", int_t, 0, "Size of the output layer", 2, 1, 100);
+
# labeling parameters
-gen.add("number_of_references_used_for_labelling", int_t, 0, "used to evaluate each elevation point cloud from different POVs", 4, 1, 100);
gen.add("max_pred_std_dev_for_labelling", double_t, 0, "To give up trying to label ground points if we don't have enough confidence in our predictions", 0.3, 0.01, 1.0);
+gen.add("score_threshold", double_t, 0, "for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres", 0.8, 0.0, 1.0);
gen.add("classify_not_labeled_points_as_obstacles", bool_t, 0, "when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle", True);
-gen.add("discard_not_connected_references_for_labelling", bool_t, 0, "to exclude not connected vertices in the roadmap from the labelling step", True);
+gen.add("ground_threshold_in_not_analyzed_areas", double_t, 0, "when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and all the points above it and below this threshold will be classified as ground", 0.3, 0.01, 1.0);
# visualization and debug parameters
gen.add("measure_performance", bool_t, 0, "to measure times of inner functions", False);
diff --git a/include/aos.hpp b/include/aos.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d34f9ec9800af6b0ccdbec8357d04f126812b6ad
--- /dev/null
+++ b/include/aos.hpp
@@ -0,0 +1,701 @@
+#ifndef TRAVEL_OSEG_H
+#define TRAVEL_OSEG_H
+//
+// Created by Minho Oh & Euigon Jung on 1/31/22.
+// We really appreciate Hyungtae Lim and Prof. Hyun Myung! :)
+//
+#pragma once
+#include <vector>
+#include <algorithm>
+#include <iostream>
+#include <list>
+#include <numeric>
+#include <random>
+#include <chrono>
+#include <forward_list>
+#include <boost/optional.hpp>
+
+#include <ros/ros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/io.h>
+
+using namespace std;
+
+namespace travel {
+
+ class AOSNode {
+ public:
+ uint start;
+ uint end;
+ int label;
+
+ AOSNode() : label(-1) {}
+ };
+
+ class Point {
+ public:
+ float x,y,z;
+ uint idx;
+ bool valid = false;
+ int label;
+ };
+
+ template <typename T>
+ class ObjectCluster {
+ private:
+ uint16_t max_label_;
+ vector<forward_list<Point*>> clusters_;
+ forward_list<Point*> empty_;
+ vector<int> valid_cnt_;
+ vector<vector<AOSNode>> nodes_;
+ vector<float> vert_angles_;
+ vector<vector<Point>> range_mat_;
+ vector<vector<Point>> emptyrange_mat_;
+
+ // parameters
+ int VERT_SCAN;
+ int HORZ_SCAN;
+ float MIN_RANGE;
+ float MAX_RANGE;
+ float HORZ_MERGE_THRES;
+ float VERT_MERGE_THRES;
+ int VERT_SCAN_SIZE;
+ int HORZ_SCAN_SIZE;
+ int HORZ_SKIP_SIZE;
+ int HORZ_EXTENSION_SIZE;
+ int DOWNSAMPLE;
+ float MIN_VERT_ANGLE;
+ float MAX_VERT_ANGLE;
+ boost::optional<int> MIN_CLUSTER_SIZE;
+ boost::optional<int> MAX_CLUSTER_SIZE;
+
+ // test
+ // vector<vector<vector<std::pair<float, float>>>> point_angle_;
+
+ public:
+ ObjectCluster() {
+ empty_ = {};
+ }
+
+ ~ObjectCluster() {}
+
+ void setParams(int _vert_scan, int _horz_scan, float _min_range, float _max_range,
+ float _min_vert_angle, float _max_vert_angle,
+ float _horz_merge_thres, float _vert_merge_thres, int _vert_scan_size,
+ int _horz_scan_size, int _horz_extension_size, int _horz_skip_size,
+ int _downsample,
+ boost::optional<int> _min_cluster_size=boost::none,
+ boost::optional<int> _max_cluster_size=boost::none) {
+
+ std::cout<<""<<std::endl;
+ ROS_WARN("Set ObjectSeg Parameters");
+
+ VERT_SCAN = _vert_scan;
+ ROS_WARN("vert scan: %d", VERT_SCAN);
+
+ HORZ_SCAN = _horz_scan;
+ ROS_WARN("horz scan: %d", HORZ_SCAN);
+
+ MIN_RANGE = _min_range;
+ ROS_WARN("min range: %f", MIN_RANGE);
+
+ MAX_RANGE = _max_range;
+ ROS_WARN("max range: %f", MAX_RANGE);
+
+ HORZ_MERGE_THRES = _horz_merge_thres;
+ ROS_WARN("horz merge thres: %f", HORZ_MERGE_THRES);
+
+ VERT_MERGE_THRES = _vert_merge_thres;
+ ROS_WARN("vert merge thres: %f", VERT_MERGE_THRES);
+
+ VERT_SCAN_SIZE = _vert_scan_size;
+ ROS_WARN("vert scan size: %d", VERT_SCAN_SIZE);
+
+ HORZ_SKIP_SIZE = _horz_skip_size;
+ ROS_WARN("horz skip size: %d", HORZ_SKIP_SIZE);
+
+ HORZ_SCAN_SIZE = _horz_scan_size;
+ ROS_WARN("horz scan size: %d", HORZ_SCAN_SIZE);
+
+ HORZ_EXTENSION_SIZE = _horz_extension_size;
+ ROS_WARN("horz extension size: %d", HORZ_EXTENSION_SIZE);
+
+ DOWNSAMPLE = _downsample;
+ ROS_WARN("downsample: %d", DOWNSAMPLE);
+
+ MAX_VERT_ANGLE = _max_vert_angle;
+ MIN_VERT_ANGLE = _min_vert_angle;
+ ROS_WARN("Max vertical angle : %f", MAX_VERT_ANGLE);
+ ROS_WARN("Min vertical angle : %f", MIN_VERT_ANGLE);
+
+ float resolution = (float) (MAX_VERT_ANGLE - MIN_VERT_ANGLE) / (float)(VERT_SCAN-1);
+ ROS_WARN("Vertical resolution: %f", resolution);
+
+
+ for(int i = 0; i < _vert_scan; i++)
+ vert_angles_.push_back(_min_vert_angle + i*resolution);
+
+ range_mat_.resize(VERT_SCAN, vector<Point>(HORZ_SCAN));
+
+ valid_cnt_.resize(VERT_SCAN, 0);
+ nodes_.resize(VERT_SCAN, vector<AOSNode>());
+
+ if (_min_cluster_size)
+ MIN_CLUSTER_SIZE = _min_cluster_size;
+
+ if (_max_cluster_size)
+ MAX_CLUSTER_SIZE = _max_cluster_size;
+ }
+
+ void segmentObjects(boost::shared_ptr<pcl::PointCloud<T>> cloud_in,
+ boost::shared_ptr<pcl::PointCloud<T>> cloud_out,
+ vector<float> *vert_angles=nullptr) {
+ // 0. reset
+ max_label_ = 1;
+ clusters_.clear();
+ clusters_.push_back(empty_);
+ clusters_.push_back(empty_);
+ std::fill(valid_cnt_.begin(), valid_cnt_.end(), 0);
+ std::for_each(range_mat_.begin(), range_mat_.end(), [](vector<Point>& inner_vec) {
+ std::fill(inner_vec.begin(), inner_vec.end(), Point());
+ });
+ // 1. do spherical projection
+ auto start = chrono::steady_clock::now();
+ sphericalProjection(cloud_in);
+ auto end = chrono::steady_clock::now();
+ // cloud_out->points.resize(cloud_in->points.size());
+ printf("Creating range map: %ld ms\n", chrono::duration_cast<chrono::milliseconds>(end-start).count());
+
+ // 2. begin clustering
+ start = chrono::steady_clock::now();
+ for (int channel = 0; channel < VERT_SCAN; channel++) {
+ auto horz_start = chrono::steady_clock::now();
+ horizontalUpdate(channel);
+ auto horz_end = chrono::steady_clock::now();
+ auto vert_start = chrono::steady_clock::now();
+ verticalUpdate(channel);
+ auto vert_end = chrono::steady_clock::now();
+ // printf("Channel %d: pt count: %d, node count: %d, horz: %ld ms, vert: %ld ms\n", channel,
+ // valid_cnt_[channel], nodes_[channel].size(),
+ // chrono::duration_cast<chrono::milliseconds>(horz_end-horz_start).count(),
+ // chrono::duration_cast<chrono::milliseconds>(vert_end-vert_start).count());
+ }
+ end = chrono::steady_clock::now();
+ printf("Processing clustering: %ld ms\n", chrono::duration_cast<chrono::milliseconds>(end-start).count());
+
+ // 3. post-processing (clusters -> labels)
+ start = chrono::steady_clock::now();
+ labelPointcloud(cloud_in, cloud_out);
+ end = chrono::steady_clock::now();
+ printf("Post-processing: %ld ms\n", chrono::duration_cast<chrono::milliseconds>(end-start).count());
+ }
+
+ void sphericalProjection(boost::shared_ptr<pcl::PointCloud<T>> cloud_in) {
+ boost::shared_ptr<pcl::PointCloud<T>> valid_cloud = boost::make_shared<pcl::PointCloud<T>>();
+ valid_cloud->points.reserve(cloud_in->points.size());
+ int ring_idx = -1, row_idx = -1, col_idx = -1;
+ float range;
+ Point point;
+ int range_chcker = 0;
+ int valid_checker = 0;
+ int count_range, count_downsample, count_out_of_row, count_out_of_col, count_out_of_mat;
+ count_range = count_downsample = count_out_of_row = count_out_of_col = count_out_of_mat = 0;
+ int count_pass = 0;
+ for (size_t i = 0; i < cloud_in->points.size(); i++) {
+
+ range = getRange(cloud_in->points[i]);
+ if (range < MIN_RANGE || range > MAX_RANGE){
+ count_range++;
+ continue;
+ }
+ row_idx = getRowIdx(cloud_in->points[i]);
+ if (row_idx % DOWNSAMPLE != 0) {
+ count_downsample++;
+ continue;
+ }
+ if (row_idx < 0 || row_idx >= VERT_SCAN) {
+ count_out_of_row++;
+ continue;
+ }
+ col_idx = getColIdx(cloud_in->points[i]);
+ if (col_idx < 0 || col_idx >= HORZ_SCAN) {
+ count_out_of_col++;
+ continue;
+ }
+ if (range_mat_[row_idx][col_idx].valid){
+ count_out_of_mat ++;
+ continue;
+ } else {
+ count_pass++;
+ point.x = cloud_in->points[i].x;
+ point.y = cloud_in->points[i].y;
+ point.z = cloud_in->points[i].z;
+ point.valid = true;
+ point.idx = valid_cloud->points.size();
+ valid_cloud->points.push_back(cloud_in->points[i]);
+ range_mat_[row_idx][col_idx] = point;
+ assert(range_mat_[row_idx][col_idx].valid == true);
+ valid_cnt_[row_idx]++;
+ }
+ }
+ printf("Input cloud size: %d\n", (int)cloud_in->points.size());
+ printf("Projected cloud size: %d\n", (int)valid_cloud->points.size());
+ *cloud_in = *valid_cloud;
+ }
+
+ void labelPointcloud(boost::shared_ptr<pcl::PointCloud<T>> cloud_in,
+ boost::shared_ptr<pcl::PointCloud<T>> cloud_out) {
+ uint cnt = 0;
+ uint pt_cnt = 0;
+ uint max = 2000;
+ cloud_out->points.reserve(cloud_in->points.size());
+
+ // generate random number
+ std::vector<size_t> valid_indices;
+ for (size_t i = 0; i < clusters_.size(); i++) {
+ if (std::distance(clusters_[i].begin(), clusters_[i].end()) > 0) {
+ valid_indices.push_back(i);
+ cnt++;
+ }
+ }
+ std::list<uint16_t> l(cnt);
+ std::iota(l.begin(), l.end(), 1);
+ std::vector<std::list<uint16_t>::iterator> v(l.size());
+ std::iota(v.begin(), v.end(), l.begin());
+ std::shuffle(v.begin(), v.end(), std::mt19937{std::random_device{}()});
+
+ for (size_t i = 0; i < valid_indices.size(); i++) {
+ uint16_t label = *v[i];
+ size_t idx = valid_indices[i];
+ int point_size = std::distance(clusters_[idx].begin(), clusters_[idx].end());
+ if (MIN_CLUSTER_SIZE && MAX_CLUSTER_SIZE) {
+ if (point_size < MIN_CLUSTER_SIZE || point_size > MAX_CLUSTER_SIZE)
+ continue;
+ }
+
+ for (auto &p : clusters_[idx]) {
+ if (p->valid) {
+ assert(cloud_in->points[p->idx].x == p->x);
+ assert(cloud_in->points[p->idx].y == p->y);
+ assert(cloud_in->points[p->idx].z == p->z);
+ T point = cloud_in->points[p->idx];
+ point.intensity = label;
+ cloud_out->points.push_back(point);
+ pt_cnt++;
+ } else {
+ ROS_ERROR("point invalid");
+ }
+ }
+ }
+ // assert(cloud_in->points.size() == cloud_out->points.size());
+ printf("# of cluster: %d, point: %d\n", cnt, pt_cnt);
+ }
+
+ void horizontalUpdate(int channel) {
+ nodes_[channel].clear();
+ // no point in the channel
+ if (valid_cnt_[channel] <= 0){
+ return;
+ }
+
+ int first_valid_idx = -1;
+ int last_valid_idx = -1;
+ int start_pos = -1;
+ int pre_pos = -1;
+ int end_pos = -1;
+
+ AOSNode node;
+ bool first_node = false;
+ for(int j = 0; j < HORZ_SCAN; j++) {
+ if (!range_mat_[channel][j].valid)
+ continue;
+ if (!first_node) {
+ first_node = true;
+ // update index
+ first_valid_idx = j;
+ start_pos = j;
+ pre_pos = j;
+ // update label
+ max_label_++;
+ clusters_.push_back(empty_);
+ range_mat_[channel][j].label = max_label_;
+ // push a new node
+ node.start = start_pos;
+ node.end = j;
+ node.label = range_mat_[channel][j].label;
+ nodes_[channel].push_back(node);
+ clusters_[node.label].insert_after(clusters_[node.label].cbefore_begin(), &range_mat_[channel][j]);
+ } else {
+ auto &cur_pt = range_mat_[channel][j];
+ auto &pre_pt = range_mat_[channel][pre_pos];
+ if (pointDistance(cur_pt, pre_pt) < HORZ_MERGE_THRES) {
+ // update existing node
+ pre_pos = j;
+ cur_pt.label = pre_pt.label;
+ nodes_[channel].back().end = j;
+ } else {
+ // update index
+ start_pos = j;
+ pre_pos = j;
+ // update label
+ max_label_++;
+ clusters_.push_back(empty_);
+ cur_pt.label = max_label_;
+ // push new node
+ node.start = start_pos;
+ node.end = j;
+ node.label = range_mat_[channel][j].label;
+ nodes_[channel].push_back(node);
+ assert(range_mat_[channel][j].label == cur_pt.label);
+ }
+ clusters_[cur_pt.label].insert_after(clusters_[cur_pt.label].cbefore_begin(), &range_mat_[channel][j]);
+ }
+ }
+ last_valid_idx = pre_pos;
+
+ // merge last and first points
+ if (nodes_[channel].size() > 2) {
+ auto &p_0 = range_mat_[channel][first_valid_idx];
+ auto &p_l = range_mat_[channel][last_valid_idx];
+ if (pointDistance(p_0, p_l) < HORZ_MERGE_THRES) {
+ if (p_0.label == 0)
+ printf("Ring merge to label 0\n");
+ if (p_0.label != p_l.label) {
+ nodes_[channel].back().label = p_0.label;
+ mergeClusters(p_l.label, p_0.label);
+ }
+ }
+ }
+
+ // merge skipped nodes due to occlusion
+ if (nodes_[channel].size() > 2) {
+ uint16_t cur_label = 0, target_label = 0;
+ for (size_t i = 0; i < nodes_[channel].size()-1; i++){
+ for (size_t j = i+1; j < nodes_[channel].size(); j++) {
+ auto &node_i = nodes_[channel][i];
+ auto &node_j = nodes_[channel][j];
+
+ if (node_i.label == node_j.label)
+ continue;
+
+ int end_idx = node_i.end;
+ int start_idx = node_j.start;
+ float dist = pointDistance(range_mat_[channel][end_idx], range_mat_[channel][start_idx]);
+ if (dist < HORZ_MERGE_THRES) {
+ if (node_i.label > node_j.label) {
+ target_label = node_j.label;
+ cur_label = node_i.label;
+ node_i.label = target_label;
+ } else {
+ target_label = node_i.label;
+ cur_label = node_j.label;
+ node_j.label = target_label;
+ }
+ mergeClusters(cur_label, target_label);
+ // if (DEBUG)
+ // printf("merge two labels despite occlusion: %d %d\n", cur_label, target_label);
+ }
+ if (j-i >= HORZ_SKIP_SIZE)
+ break;
+ }
+ }
+ }
+ }
+
+ void verticalUpdate(int channel) {
+ // Iterate each point of this channel to update the labels.
+ int point_size = valid_cnt_[channel];
+ // Current scan line is emtpy, do nothing.
+ if(point_size == 0) return;
+ // Skip first scan line
+ if(channel == 0) return;
+
+ int prev_channel = channel - 1;
+
+ for(int n = 0; n < nodes_[channel].size(); n++) {
+
+ auto &cur_node = nodes_[channel][n];
+
+ for (int l = prev_channel; l >= channel - VERT_SCAN_SIZE; l -= 1) {
+
+ if (l < 0) // out of range
+ break;
+
+ if (valid_cnt_[l] == 0) {
+ // if (DEBUG)
+ // printf("No valid points in the channel\n");
+ continue;
+ }
+
+ // binary search lower bound
+ // lower_bnd inclusive
+ int N = nodes_[l].size();
+ int first = 0;
+ int last = N - 1;
+ int lower_bnd = 0;
+ int mid = 0;
+
+ while (first <= last) {
+ mid = (first + last) / 2;
+ auto &prev_node = nodes_[l][mid];
+ if (overlap(cur_node, prev_node) || cur_node.end < prev_node.start){
+ lower_bnd = mid;
+ last = mid - 1;
+ } else {
+ first = mid + 1;
+ }
+ }
+
+ // binary search upper bound
+ // exclusive but gives -1 if end of list
+ first = 0;
+ last = N - 1;
+ mid = 0;
+ int upper_bnd = 0;
+
+ while (first <= last) {
+ mid = (first + last) / 2;
+ auto &prev_node = nodes_[l][mid];
+ if (overlap(cur_node, prev_node) || prev_node.end < cur_node.start) {
+ upper_bnd = mid;
+ first = mid + 1;
+ } else {
+ last = mid - 1;
+ }
+ }
+ upper_bnd = upper_bnd + 1;
+
+ // loop through overlapped nodes
+ for (size_t idx = lower_bnd; idx < upper_bnd; idx++) {
+
+ auto &ovl_node = nodes_[l][idx];
+
+ if (ovl_node.label == cur_node.label) {
+ // if (DEBUG)
+ // printf("same label: %d, %d\n", ovl_node.label, cur_node.label);
+ continue;
+ }
+
+ // if (DEBUG)
+ // printf("overlapped: %d, %d\n", ovl_node.start, ovl_node.end);
+
+ int iter_start_idx = -1;
+ int iter_end_idx = -1;
+
+ if (ovl_node.start <= cur_node.start && cur_node.end <= ovl_node.end) {
+ // cur_node inside prev_node
+ iter_start_idx = cur_node.start;
+ iter_end_idx = cur_node.end;
+ } else if (cur_node.start <= ovl_node.start && ovl_node.end <= cur_node.end) {
+ // prev_node inside cur_node
+ iter_start_idx = ovl_node.start;
+ iter_end_idx = ovl_node.end;
+ } else if (cur_node.start < ovl_node.start && cur_node.end >= ovl_node.start && cur_node.end <= ovl_node.end) {
+ // tail of cur_node with head of prev_node
+ iter_start_idx = ovl_node.start;
+ iter_end_idx = cur_node.end;
+ } else if (ovl_node.start <= cur_node.start && cur_node.start <= ovl_node.end && cur_node.end > ovl_node.end) {
+ // head of cur_node with tail of prev_node
+ iter_start_idx = cur_node.start;
+ iter_end_idx = ovl_node.end;
+ } else {
+ // overlapped within search window size, use euclidean distance directly
+ if (ovl_node.end < cur_node.start) {
+ if (nodeDistance(cur_node, ovl_node, range_mat_[channel][cur_node.start], range_mat_[l][ovl_node.end])) {
+ // if (DEBUG) {
+ // printf("Merge by euclidean distance: cur: %f;%f;%f, prev: %f;%f;%f\n",
+ // range_mat_[channel][cur_node.start].x, range_mat_[channel][cur_node.start].y, range_mat_[channel][cur_node.start].z,
+ // range_mat_[l][ovl_node.end].x, range_mat_[l][ovl_node.end].y, range_mat_[l][ovl_node.end].z);
+ // }
+ }
+ } else if (cur_node.end < ovl_node.start) {
+ if (nodeDistance(cur_node, ovl_node, range_mat_[channel][cur_node.end], range_mat_[l][ovl_node.start])) {
+ // if (DEBUG) {
+ // printf("Merge by euclidean distance: cur: %f;%f;%f, prev: %f;%f;%f\n",
+ // range_mat_[channel][cur_node.end].x, range_mat_[channel][cur_node.end].y, range_mat_[channel][cur_node.end].z,
+ // range_mat_[l][ovl_node.start].x, range_mat_[l][ovl_node.start].y, range_mat_[l][ovl_node.start].z);
+ // }
+ }
+ }
+ continue;
+ }
+
+ // if (DEBUG)
+ // printf("overlapping: %d %d\n", iter_start_idx, iter_end_idx);
+
+ // iterate through overlapping indices
+ uint16_t cur_label = 0, target_label = 0;
+ bool merged = false;
+ int cur_start_left = iter_start_idx;
+ int cur_start_right = iter_start_idx;
+ int cur_end_left = iter_end_idx;
+ int cur_end_right = iter_end_idx;
+
+ while (1) {
+ if (cur_start_right > cur_end_left && cur_start_left < iter_start_idx - HORZ_SCAN_SIZE && cur_end_right > iter_end_idx + HORZ_SCAN_SIZE) // end of search
+ break;
+ if (mergeNodes(cur_node, ovl_node, channel, l, cur_start_left))
+ break;
+ if (cur_start_left != cur_end_left) { // more than one overlapping cur_node point
+ if (mergeNodes(cur_node, ovl_node, channel, l, cur_end_left))
+ break;
+ }
+ if (cur_start_left != cur_start_right) { // not the first iteration
+ if (mergeNodes(cur_node, ovl_node, channel, l, cur_start_right))
+ break;
+ }
+ if (cur_end_left != cur_end_right) { // not the first iteration
+ if (mergeNodes(cur_node, ovl_node, channel, l, cur_end_right))
+ break;
+ }
+ cur_start_left--;
+ cur_start_right++;
+ cur_end_left--;
+ cur_end_right++;
+ }
+ }
+ }
+ }
+ }
+
+ bool overlap(AOSNode &first_node, AOSNode &second_node) {
+ int new_start = first_node.start - HORZ_EXTENSION_SIZE;
+ int new_end = first_node.end + HORZ_EXTENSION_SIZE;
+ if (new_start <= second_node.start && second_node.start <= new_end)
+ return true;
+ else if (new_start <= second_node.end && second_node.end <= new_end)
+ return true;
+ else if (second_node.start <= new_start && new_end <= second_node.end)
+ return true;
+ return false;
+ }
+
+ bool mergeNodes(AOSNode &first_node, AOSNode &second_node, int cur_channel, int prev_channel, int query_idx) {
+ if (query_idx >= first_node.start && query_idx <= first_node.end) {
+ if (range_mat_[cur_channel][query_idx].valid) {
+ int left_idx = query_idx;
+ int right_idx = query_idx;
+ while (1) {
+ if (left_idx <= query_idx - HORZ_SCAN_SIZE && right_idx >= query_idx + HORZ_SCAN_SIZE)
+ break;
+
+ if (left_idx >= second_node.start && left_idx <= second_node.end && range_mat_[prev_channel][left_idx].valid) {
+ if (nodeDistance(first_node, second_node, range_mat_[cur_channel][query_idx], range_mat_[prev_channel][left_idx])) {
+ // if (DEBUG)
+ // printf("query: %d, left_idx: %d\n", query_idx, left_idx);
+ return true;
+ }
+ }
+
+ if (right_idx <= second_node.end && right_idx >= second_node.start && range_mat_[prev_channel][right_idx].valid) {
+ if (nodeDistance(first_node, second_node, range_mat_[cur_channel][query_idx], range_mat_[prev_channel][right_idx])) {
+ // if (DEBUG)
+ // printf("query: %d, right_idx: %d\n", query_idx, right_idx);
+ return true;
+ }
+ }
+ left_idx--;
+ right_idx++;
+ }
+ }
+ }
+ return false;
+ }
+
+ float pointDistance(Point pt1, Point pt2) {
+ return sqrt((pt1.x - pt2.x)*(pt1.x-pt2.x) + (pt1.y-pt2.y)*(pt1.y-pt2.y));
+ }
+
+ bool nodeDistance(AOSNode &first_node, AOSNode &second_node, Point &first_point, Point &second_point) {
+ uint16_t cur_label, target_label = 0;
+
+ if (first_node.label == second_node.label)
+ return false;
+
+ if (pointDistance(first_point, second_point) < VERT_MERGE_THRES) {
+ // if (DEBUG) {
+ // printf("cur: %f;%f;%f, prev: %f;%f;%f distance: %f\n", first_point.x, first_point.y, first_point.z, second_point.x, second_point.y, second_point.z, pointDistance(first_point, second_point));
+ // printf("Two nodes merged: %d, %d\n", first_node.label, second_node.label);
+ // }
+
+ if (first_node.label > second_node.label) {
+ cur_label = first_node.label;
+ target_label = second_node.label;
+ first_node.label = target_label;
+ } else {
+ cur_label = second_node.label;
+ target_label = first_node.label;
+ second_node.label = target_label;
+ }
+ mergeClusters(cur_label, target_label);
+ return true;
+ }
+ return false;
+ }
+
+ float getRange(T pt) {
+ return sqrt(pt.x*pt.x+pt.y*pt.y+pt.z*pt.z);
+ }
+
+ int getRowIdx(T pt) {
+ float angle = atan2(pt.z, sqrt(pt.x*pt.x+pt.y*pt.y))*180/M_PI;
+ auto iter_geq = std::lower_bound(vert_angles_.begin(), vert_angles_.end(), angle);
+ int row_idx;
+
+ if (iter_geq == vert_angles_.begin()) {
+ row_idx = 0;
+ } else {
+ float a = *(iter_geq - 1);
+ float b = *(iter_geq);
+ if (fabs(angle-a) < fabs(angle-b)){
+ row_idx = iter_geq - vert_angles_.begin() - 1;
+ } else {
+ row_idx = iter_geq - vert_angles_.begin();
+ }
+ }
+ return row_idx;
+ }
+
+ int getRowIdx(T pt, vector<float> vert_angles) {
+ float angle = atan2(pt.z, sqrt(pt.x*pt.x+pt.y*pt.y))*180/M_PI;
+ auto iter_geq = std::lower_bound(vert_angles.begin(), vert_angles.end(), angle);
+ int row_idx;
+
+ if (iter_geq == vert_angles.begin()) {
+ row_idx = 0;
+ } else {
+ float a = *(iter_geq - 1);
+ float b = *(iter_geq);
+ if (fabs(angle-a) < fabs(angle-b)){
+ row_idx = iter_geq - vert_angles.begin() - 1;
+ } else {
+ row_idx = iter_geq - vert_angles.begin();
+ }
+ }
+ return row_idx;
+ }
+
+ int getColIdx(T pt) {
+ float horizonAngle = atan2(pt.x, pt.y) * 180 / M_PI;
+ static float ang_res_x = 360.0/float(HORZ_SCAN);
+ int col_idx = -round((horizonAngle-90.0)/ang_res_x) + HORZ_SCAN/2;
+ if (col_idx >= HORZ_SCAN)
+ col_idx -= HORZ_SCAN;
+ return col_idx;
+ }
+
+ void mergeClusters(uint16_t cur_label, uint16_t target_label) {
+ if(cur_label == 0 || target_label == 0){
+ printf("Error merging runs cur_label:%u target_label:%u", cur_label, target_label);
+ }
+ for(auto &p : clusters_[cur_label]){
+ p->label = target_label;
+ }
+ clusters_[target_label].insert_after(clusters_[target_label].cbefore_begin(), clusters_[cur_label].begin(),clusters_[cur_label].end() );
+ clusters_[cur_label].clear();
+ }
+ };
+}
+
+#endif
\ No newline at end of file
diff --git a/include/tgs.hpp b/include/tgs.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..99746f90981556222b38c2e78d52b3fe4ecae89f
--- /dev/null
+++ b/include/tgs.hpp
@@ -0,0 +1,1643 @@
+#ifndef TRAVEL_GSEG_H
+#define TRAVEL_GSEG_H
+//
+// Created by Minho Oh & Euigon Jung on 1/31/22.
+// We really appreciate Hyungtae Lim and Prof. Hyun Myung! :)
+//
+#include <iostream>
+#include <Eigen/Dense>
+#include <vector>
+#include <random>
+#include <mutex>
+#include <thread>
+#include <chrono>
+#include <math.h>
+#include <fstream>
+#include <memory>
+#include <signal.h>
+
+#include <ros/ros.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/filter.h>
+#include <pcl/common/centroid.h>
+#include <pcl/filters/voxel_grid.h>
+#include <tf/transform_listener.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
+#include <jsk_recognition_msgs/PolygonArray.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <opencv2/opencv.hpp>
+
+namespace travel {
+
+ #define PTCLOUD_SIZE 132000
+ #define NODEWISE_PTCLOUDSIZE 5000
+
+ #define UNKNOWN 1
+ #define NONGROUND 2
+ #define GROUND 3
+
+ using Eigen::MatrixXf;
+ using Eigen::JacobiSVD;
+ using Eigen::VectorXf;
+
+ template <typename PointType>
+ bool point_z_cmp(PointType a, PointType b){
+ return a.z<b.z;
+ }
+
+ struct TriGridIdx {
+ int row, col, tri;
+ };
+
+ struct TriGridEdge {
+ std::pair<TriGridIdx, TriGridIdx> Pair;
+ bool is_traversable;
+ };
+
+ template <typename PointType>
+ struct TriGridNode {
+ int node_type;
+ pcl::PointCloud<PointType> ptCloud;
+
+ bool is_curr_data;
+
+ // planar model
+ Eigen::Vector3f normal;
+ Eigen::Vector3f mean_pt;
+ double d;
+
+ Eigen::Vector3f singular_values;
+ Eigen::Matrix3f eigen_vectors;
+ double weight;
+
+ double th_dist_d;
+ double th_outlier_d;
+
+ // graph_searching
+ bool need_recheck;
+ bool is_visited;
+ bool is_rejection;
+ int check_life;
+ int depth;
+ };
+
+ struct TriGridCorner {
+ double x, y;
+ std::vector<double> zs;
+ std::vector<double> weights;
+ };
+
+ template <typename PointType>
+ using GridNode = std::vector<TriGridNode<PointType>>;
+
+ template <typename PointType>
+ using TriGridField = std::vector<std::vector<GridNode<PointType>>>;
+
+ template <typename PointType>
+ class TravelGroundSeg{
+ private:
+ // ros::NodeHandle node_handle_;
+ pcl::PCLHeader cloud_header_;
+ std_msgs::Header msg_header_;
+
+ // ros::Publisher pub_trigrid_nodes_;
+ // ros::Publisher pub_trigrid_edges_;
+ // ros::Publisher pub_trigrid_corners_;
+ // ros::Publisher pub_tgseg_ground_cloud;
+ // ros::Publisher pub_tgseg_nonground_cloud;
+ // ros::Publisher pub_tgseg_outliers_cloud;
+
+ jsk_recognition_msgs::PolygonArray viz_trigrid_polygons_;
+ visualization_msgs::Marker viz_trigrid_edges_;
+ pcl::PointCloud<pcl::PointXYZ> viz_trigrid_corners_;
+
+ bool REFINE_MODE_;
+ bool VIZ_MDOE_;
+
+ double MAX_RANGE_;
+ double MIN_RANGE_;
+ double TGF_RESOLUTION_;
+
+ int NUM_ITER_;
+ int NUM_LRP_;
+ int NUM_MIN_POINTS_;
+
+ double TH_SEEDS_;
+ double TH_DIST_;
+ double TH_OUTLIER_;
+
+ double TH_NORMAL_;
+ double TH_WEIGHT_;
+ double TH_LCC_NORMAL_SIMILARITY_;
+ double TH_LCC_PLANAR_MODEL_DIST_;
+ double TH_OBSTACLE_HEIGHT_;
+
+ TriGridField<PointType> trigrid_field_;
+ std::vector<TriGridEdge> trigrid_edges_;
+ std::vector<std::vector<TriGridCorner>> trigrid_corners_;
+ std::vector<std::vector<TriGridCorner>> trigrid_centers_;
+
+ pcl::PointCloud<PointType> empty_cloud_;
+ TriGridNode<PointType> empty_trigrid_node_;
+ GridNode<PointType> empty_grid_nodes_;
+ TriGridCorner empty_trigrid_corner_;
+ TriGridCorner empty_trigrid_center_;
+
+ pcl::PointCloud<PointType> ptCloud_tgfwise_ground_;
+ pcl::PointCloud<PointType> ptCloud_tgfwise_nonground_;
+ pcl::PointCloud<PointType> ptCloud_tgfwise_outliers_;
+ pcl::PointCloud<PointType> ptCloud_tgfwise_obstacle_;
+ pcl::PointCloud<PointType> ptCloud_nodewise_ground_;
+ pcl::PointCloud<PointType> ptCloud_nodewise_nonground_;
+ pcl::PointCloud<PointType> ptCloud_nodewise_outliers_;
+ pcl::PointCloud<PointType> ptCloud_nodewise_obstacle_;
+
+ public:
+ // TravelGroundSeg(ros::NodeHandle* nh):node_handle_(*nh){
+ TravelGroundSeg(){
+ // Init ROS related
+ ROS_INFO("Inititalizing Traversable Ground Segmentation...");
+
+ // pub_trigrid_nodes_ = node_handle_.advertise<jsk_recognition_msgs::PolygonArray>("/travelgseg/nodes", 1);
+ // pub_trigrid_edges_ = node_handle_.advertise<visualization_msgs::Marker>("/travelgseg/edges", 1);
+ // pub_trigrid_corners_= node_handle_.advertise<pcl::PointCloud<pcl::PointXYZ>>("/travelgseg/corners", 1);
+
+ // pub_tgseg_ground_cloud = node_handle_.advertise<sensor_msgs::PointCloud2>("/travelgseg/ground_cloud", 1);
+ // pub_tgseg_nonground_cloud = node_handle_.advertise<sensor_msgs::PointCloud2>("/travelgseg/nonground_cloud", 1);
+ // pub_tgseg_outliers_cloud = node_handle_.advertise<sensor_msgs::PointCloud2>("/travelgseg/outlier_cloud", 1);
+ };
+
+ void setParams(const double max_range, const double min_range, const double resolution,
+ const int num_iter, const int num_lpr, const int num_min_pts, const double th_seeds,
+ const double th_dist, const double th_outlier, const double th_normal, const double th_weight,
+ const double th_lcc_normal_similiarity, const double th_lcc_planar_model_dist, const double th_obstacle,
+ const bool refine_mode, const bool visualization) {
+ std::cout<<""<<std::endl;
+ ROS_INFO("Set TRAVEL_GSEG Parameters");
+
+ std::cout<<"Just about to assign the max_range value = " << max_range <<std::endl;
+ std::cout<<"to the variable MAX_RANGE_ = " << MAX_RANGE_ <<std::endl;
+
+ MAX_RANGE_ = max_range;
+ ROS_INFO("Max Range: %f", MAX_RANGE_);
+
+ MIN_RANGE_ = min_range;
+ ROS_INFO("Min Range: %f", MIN_RANGE_);
+
+ TGF_RESOLUTION_ = resolution;
+ ROS_INFO("Resolution: %f", TGF_RESOLUTION_);
+
+ NUM_ITER_ = num_iter;
+ ROS_INFO("Num of Iteration: %d", NUM_ITER_);
+
+ NUM_LRP_ = num_lpr;
+ ROS_INFO("Num of LPR: %d", NUM_LRP_);
+
+ NUM_MIN_POINTS_ = num_min_pts;
+ ROS_INFO("Num of min. points: %d", NUM_MIN_POINTS_);
+
+ TH_SEEDS_ = th_seeds;
+ ROS_INFO("Seeds Threshold: %f", TH_SEEDS_);
+
+ TH_DIST_ = th_dist;
+ ROS_INFO("Distance Threshold: %f", TH_DIST_);
+
+ TH_OUTLIER_ = th_outlier;
+ ROS_INFO("Outlier Threshold: %f", TH_OUTLIER_);
+
+ TH_NORMAL_ = th_normal;
+ ROS_INFO("Normal Threshold: %f", TH_NORMAL_);
+
+ TH_WEIGHT_ = th_weight;
+ ROS_INFO("Node Weight Threshold: %f", TH_WEIGHT_);
+
+ TH_LCC_NORMAL_SIMILARITY_ = th_lcc_normal_similiarity;
+ ROS_INFO("LCC Normal Similarity: %f", TH_LCC_NORMAL_SIMILARITY_);
+
+ TH_LCC_PLANAR_MODEL_DIST_ = th_lcc_planar_model_dist;
+ ROS_INFO("LCC Plane Distance : %f", TH_LCC_PLANAR_MODEL_DIST_);
+
+ TH_OBSTACLE_HEIGHT_ = th_obstacle;
+ ROS_INFO("Obstacle Max Height : %f", TH_OBSTACLE_HEIGHT_);
+
+ REFINE_MODE_ = refine_mode;
+ VIZ_MDOE_ = visualization;
+
+ ROS_INFO("Set TGF Parameters");
+ initTriGridField(trigrid_field_);
+ initTriGridCorners(trigrid_corners_, trigrid_centers_);
+
+ ptCloud_tgfwise_ground_.clear();
+ ptCloud_tgfwise_ground_.reserve(PTCLOUD_SIZE);
+ ptCloud_tgfwise_nonground_.clear();
+ ptCloud_tgfwise_nonground_.reserve(PTCLOUD_SIZE);
+ ptCloud_tgfwise_outliers_.clear();
+ ptCloud_tgfwise_outliers_.reserve(PTCLOUD_SIZE);
+ ptCloud_tgfwise_obstacle_.clear();
+ ptCloud_tgfwise_obstacle_.reserve(PTCLOUD_SIZE);
+
+ ptCloud_nodewise_ground_.clear();
+ ptCloud_nodewise_ground_.reserve(NODEWISE_PTCLOUDSIZE);
+ ptCloud_nodewise_nonground_.clear();
+ ptCloud_nodewise_nonground_.reserve(NODEWISE_PTCLOUDSIZE);
+ ptCloud_nodewise_outliers_.clear();
+ ptCloud_nodewise_outliers_.reserve(NODEWISE_PTCLOUDSIZE);
+ ptCloud_nodewise_obstacle_.clear();
+ ptCloud_nodewise_obstacle_.reserve(NODEWISE_PTCLOUDSIZE);
+
+ if (VIZ_MDOE_) {
+ viz_trigrid_polygons_.polygons.clear();
+ viz_trigrid_polygons_.polygons.reserve(rows_ * cols_);
+ viz_trigrid_polygons_.likelihood.clear();
+ viz_trigrid_polygons_.likelihood.reserve(rows_ * cols_);
+
+
+ viz_trigrid_edges_.ns = "trigrid_edges";
+ viz_trigrid_edges_.action = visualization_msgs::Marker::ADD;
+ viz_trigrid_edges_.type = visualization_msgs::Marker::LINE_LIST;
+ viz_trigrid_edges_.pose.orientation.w = 1.0;
+ viz_trigrid_edges_.scale.x = 0.5;
+ viz_trigrid_edges_.id = 0;
+
+ viz_trigrid_corners_.clear();
+ viz_trigrid_corners_.reserve(rows_*cols_ + (rows_+ 1)*(cols_+1));
+ }
+ };
+
+ void estimateGround(const pcl::PointCloud<PointType>& cloud_in,
+ pcl::PointCloud<PointType>& cloudGround_out,
+ pcl::PointCloud<PointType>& cloudNonground_out,
+ double& time_taken){
+
+ // 0. Init
+ static time_t start, end;
+ cloud_header_ = cloud_in.header;
+ pcl_conversions::fromPCL(cloud_header_, msg_header_);
+ // ROS_INFO("TriGrid Field-based Traversable Ground Segmentation...");
+ start = clock();
+ ptCloud_tgfwise_outliers_.clear();
+ ptCloud_tgfwise_outliers_.reserve(cloud_in.size());
+
+
+ // 1. Embed PointCloud to TriGridField
+ clearTriGridField(trigrid_field_);
+ clearTriGridCorners(trigrid_corners_, trigrid_centers_);
+
+ embedCloudToTriGridField(cloud_in, trigrid_field_);
+
+ // 2. Node-wise Terrain Modeling
+ modelNodeWiseTerrain(trigrid_field_);
+
+ // 3. Breadth-first Traversable Graph Search
+ BreadthFirstTraversableGraphSearch(trigrid_field_);
+ setTGFCornersCenters(trigrid_field_, trigrid_corners_, trigrid_centers_);
+
+ // 4. TGF-wise Traversable Terrain Model Fitting
+ if (REFINE_MODE_){
+ fitTGFWiseTraversableTerrainModel(trigrid_field_, trigrid_corners_, trigrid_centers_);
+ }
+
+ // 5. Ground Segmentation
+ segmentTGFGround(trigrid_field_, ptCloud_tgfwise_ground_, ptCloud_tgfwise_nonground_, ptCloud_tgfwise_obstacle_, ptCloud_tgfwise_outliers_);
+ cloudGround_out = ptCloud_tgfwise_ground_;
+ cloudNonground_out = ptCloud_tgfwise_nonground_;
+ cloudGround_out.header = cloudNonground_out.header = cloud_header_;
+
+ end = clock();
+ time_taken = (double)(end - start) / CLOCKS_PER_SEC;
+
+ // 6. Publish Results and Visualization
+ if (VIZ_MDOE_){
+ // publishTriGridFieldGraph();
+ // publishTriGridCorners();
+ // publishPointClouds();
+ }
+ return;
+ };
+
+ TriGridIdx getTriGridIdx(const float& x_in, const float& y_in){
+ TriGridIdx tgf_idx;
+ int r_i = (x_in - tgf_min_x)/TGF_RESOLUTION_;
+ int c_i = (y_in - tgf_min_y)/TGF_RESOLUTION_;
+ int t_i = 0;
+ double angle = atan2(y_in-(c_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_y), x_in-(r_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_x));
+
+ if (angle>=(M_PI/4) && angle <(3*M_PI/4)){
+ t_i = 1;
+ } else if (angle>=(-M_PI/4) && angle <(M_PI/4)){
+ t_i = 0;
+ } else if (angle>=(-3*M_PI/4) && angle <(-M_PI/4)){
+ t_i = 3;
+ } else{
+ t_i = 2;
+ }
+ tgf_idx.row = r_i;
+ tgf_idx.col = c_i;
+ tgf_idx.tri = t_i;
+ return tgf_idx;
+ }
+
+ TriGridNode<PointType> getTriGridNode(const float& x_in, const float& y_in){
+ TriGridNode<PointType> node;
+ TriGridIdx node_idx = getTriGridIdx(x_in, y_in);
+ node = trigrid_field_[node_idx.row][node_idx.col][node_idx.tri];
+ return node;
+ };
+
+ TriGridNode<PointType> getTriGridNode(const TriGridIdx& tgf_idx){
+ TriGridNode<PointType> node;
+ node = trigrid_field_[tgf_idx.row][tgf_idx.col][tgf_idx.tri];
+ return node;
+ };
+
+ bool is_traversable(const float& x_in, const float& y_in){
+ TriGridNode<PointType> node = getTriGridNode(x_in, y_in);
+ if (node.node_type == GROUND){
+ return true;
+ } else{
+ return false;
+ }
+ };
+
+ pcl::PointCloud<PointType> getObstaclePC(){
+ pcl::PointCloud<PointType> cloud_obstacle;
+ cloud_obstacle = ptCloud_tgfwise_obstacle_;
+ return cloud_obstacle;
+ };
+
+ private:
+ double tgf_max_x, tgf_max_y, tgf_min_x, tgf_min_y;
+ double rows_, cols_;
+
+ void initTriGridField(TriGridField<PointType>& tgf_in){
+ // ROS_INFO("Initializing TriGridField...");
+
+ tgf_max_x = MAX_RANGE_;
+ tgf_max_y = MAX_RANGE_;
+
+ tgf_min_x = -MAX_RANGE_;
+ tgf_min_y = -MAX_RANGE_;
+
+ rows_ = (int)(tgf_max_x - tgf_min_x) / TGF_RESOLUTION_;
+ cols_ = (int)(tgf_max_y - tgf_min_y) / TGF_RESOLUTION_;
+ empty_cloud_.clear();
+ empty_cloud_.reserve(PTCLOUD_SIZE);
+
+ // Set Node structure
+ empty_trigrid_node_.node_type = UNKNOWN;
+ empty_trigrid_node_.ptCloud.clear();
+ empty_trigrid_node_.ptCloud.reserve(NODEWISE_PTCLOUDSIZE);
+
+ empty_trigrid_node_.is_curr_data = false;
+ empty_trigrid_node_.need_recheck = false;
+ empty_trigrid_node_.is_visited = false;
+ empty_trigrid_node_.is_rejection = false;
+
+ empty_trigrid_node_.check_life = 10;
+ empty_trigrid_node_.depth = -1;
+
+ empty_trigrid_node_.normal;
+ empty_trigrid_node_.mean_pt;
+ empty_trigrid_node_.d = 0;
+
+ empty_trigrid_node_.singular_values;
+ empty_trigrid_node_.eigen_vectors;
+ empty_trigrid_node_.weight = 0;
+
+ empty_trigrid_node_.th_dist_d = 0;
+ empty_trigrid_node_.th_outlier_d = 0;
+
+ // Set TriGridField
+ tgf_in.clear();
+ std::vector<GridNode<PointType>> vec_gridnode;
+
+ for (int i = 0; i < 4 ; i ++)
+ empty_grid_nodes_.emplace_back(empty_trigrid_node_);
+
+ for (int i=0; i< cols_; i++){ vec_gridnode.emplace_back(empty_grid_nodes_);}
+ for (int j=0; j< rows_; j++){ tgf_in.emplace_back(vec_gridnode);}
+
+ return;
+ };
+
+ void initTriGridCorners(std::vector<std::vector<TriGridCorner>>& trigrid_corners_in,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers_in){
+ // ROS_INFO("Initializing TriGridCorners...");
+
+ // Set TriGridCorner
+ empty_trigrid_corner_.x = empty_trigrid_corner_.y = 0.0;
+ empty_trigrid_corner_.zs.clear();
+ empty_trigrid_corner_.zs.reserve(8);
+ empty_trigrid_corner_.weights.clear();
+ empty_trigrid_corner_.weights.reserve(8);
+
+ empty_trigrid_center_.x = empty_trigrid_center_.y = 0.0;
+ empty_trigrid_center_.zs.clear();
+ empty_trigrid_center_.zs.reserve(4);
+ empty_trigrid_center_.weights.clear();
+ empty_trigrid_center_.weights.reserve(4);
+
+ trigrid_corners_in.clear();
+ trigrid_centers_in.clear();
+
+ // Set Corners and Centers
+ std::vector<TriGridCorner> col_corners;
+ std::vector<TriGridCorner> col_centers;
+ for (int i=0; i< cols_; i++){
+ col_corners.emplace_back(empty_trigrid_corner_);
+ col_centers.emplace_back(empty_trigrid_center_);
+ }
+ col_corners.emplace_back(empty_trigrid_corner_);
+
+ for (int j=0; j< rows_; j++){
+ trigrid_corners_in.emplace_back(col_corners);
+ trigrid_centers_in.emplace_back(col_centers);
+ }
+ trigrid_corners_in.emplace_back(col_corners);
+
+ return;
+ };
+
+ void clearTriGridField(TriGridField<PointType> &tgf_in){
+ // ROS_INFO("Clearing TriGridField...");
+
+ for (int r_i = 0; r_i < rows_; r_i++){
+ for (int c_i = 0; c_i < cols_; c_i++){
+ tgf_in[r_i][c_i] = empty_grid_nodes_;
+ }
+ }
+ return;
+ };
+
+ void clearTriGridCorners(std::vector<std::vector<TriGridCorner>>& trigrid_corners_in,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers_in){
+ // ROS_INFO("Clearing TriGridCorners...");
+
+ TriGridCorner tmp_corner = empty_trigrid_corner_;
+ TriGridCorner tmp_center = empty_trigrid_center_;
+ for (int r_i = 0; r_i < rows_+1; r_i++){
+ for (int c_i = 0; c_i < cols_+1; c_i++){
+ tmp_corner.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; tmp_corner.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ tmp_corner.zs.clear();
+ tmp_corner.weights.clear();
+ trigrid_corners_in[r_i][c_i] = tmp_corner;
+ if (r_i < rows_ && c_i < cols_) {
+ tmp_center.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; tmp_center.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ tmp_center.zs.clear();
+ tmp_center.weights.clear();
+ trigrid_centers_in[r_i][c_i] = tmp_center;
+ }
+ }
+ }
+ return;
+ };
+
+ double xy_2Dradius(double x, double y){
+ return sqrt(x*x + y*y);
+ };
+
+ bool filterPoint(const PointType &pt_in){
+ double xy_range = xy_2Dradius(pt_in.x, pt_in.y);
+ if (xy_range >= MAX_RANGE_ || xy_range <= MIN_RANGE_) return true;
+
+ return false;
+ }
+
+ void embedCloudToTriGridField(const pcl::PointCloud<PointType>& cloud_in, TriGridField<PointType>& tgf_out) {
+ // ROS_INFO("Embedding PointCloud to TriGridField...");
+
+ for (auto const &pt: cloud_in.points){
+ if (filterPoint(pt)){
+ ptCloud_tgfwise_outliers_.points.push_back(pt);
+ continue;
+ }
+
+ int r_i = (pt.x - tgf_min_x)/TGF_RESOLUTION_;
+ int c_i = (pt.y - tgf_min_y)/TGF_RESOLUTION_;
+
+ if (r_i < 0 || r_i >= rows_ || c_i < 0 || c_i >= cols_) {
+ ptCloud_tgfwise_outliers_.points.push_back(pt);
+ continue;
+ }
+
+ double angle = atan2(pt.y-(c_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_y), pt.x-(r_i*TGF_RESOLUTION_ + TGF_RESOLUTION_/2 + tgf_min_x));
+ if (angle>=(M_PI/4) && angle <(3*M_PI/4)){
+ // left side
+ tgf_out[r_i][c_i][1].ptCloud.push_back(pt);
+ if(!tgf_out[r_i][c_i][1].is_curr_data) {tgf_out[r_i][c_i][1].is_curr_data = true;}
+ } else if (angle>=(-M_PI/4) && angle <(M_PI/4)){
+ // upper side
+ tgf_out[r_i][c_i][0].ptCloud.push_back(pt);
+ if (!tgf_out[r_i][c_i][0].is_curr_data){tgf_out[r_i][c_i][0].is_curr_data = true;}
+
+ } else if (angle>=(-3*M_PI/4) && angle <(-M_PI/4)){
+ // right side
+ tgf_out[r_i][c_i][3].ptCloud.push_back(pt);
+ if (!tgf_out[r_i][c_i][3].is_curr_data) {tgf_out[r_i][c_i][3].is_curr_data = true;}
+ } else{
+ // lower side
+ tgf_out[r_i][c_i][2].ptCloud.push_back(pt);
+ if (!tgf_out[r_i][c_i][2].is_curr_data) {tgf_out[r_i][c_i][2].is_curr_data = true;}
+ }
+ }
+
+ return;
+ };
+
+ void extractInitialSeeds(const pcl::PointCloud<PointType>& p_sorted, pcl::PointCloud<PointType>& init_seeds){
+ //function for uniform mode
+ init_seeds.points.clear();
+
+ // LPR is the mean of Low Point Representative
+ double sum = 0;
+ int cnt = 0;
+
+ // Calculate the mean height value.
+ for (int i=0; i< (int) p_sorted.points.size() && cnt<NUM_LRP_; i++){
+ sum += p_sorted.points[i].z;
+ cnt++;
+ }
+
+ double lpr_height = cnt!=0?sum/cnt:0;
+
+ for(int i=0 ; i< (int) p_sorted.points.size() ; i++){
+ if(p_sorted.points[i].z < lpr_height + TH_SEEDS_){
+ if (p_sorted.points[i].z < lpr_height-TH_OUTLIER_) continue;
+ init_seeds.points.push_back(p_sorted.points[i]);
+ }
+ }
+
+ return;
+ }
+
+ void estimatePlanarModel(const pcl::PointCloud<PointType>& ground_in, TriGridNode<PointType>& node_out) {
+
+ // function for uniform mode
+ Eigen::Matrix3f cov_;
+ Eigen::Vector4f pc_mean_;
+ pcl::computeMeanAndCovarianceMatrix(ground_in, cov_, pc_mean_);
+
+ // Singular Value Decomposition: SVD
+ Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_, Eigen::DecompositionOptions::ComputeFullU);
+
+ // Use the least singular vector as normal
+ node_out.eigen_vectors = svd.matrixU();
+ if (node_out.eigen_vectors.col(2)(2,0)<0) {
+ node_out.eigen_vectors.col(0)*= -1;
+ node_out.eigen_vectors.col(2)*= -1;
+ }
+ node_out.normal = node_out.eigen_vectors.col(2);
+ node_out.singular_values = svd.singularValues();
+
+ // mean ground seeds value
+ node_out.mean_pt = pc_mean_.head<3>();
+
+ // according to noraml.T*[x,y,z] = -d
+ node_out.d = -(node_out.normal.transpose()*node_out.mean_pt)(0,0);
+
+ // set distance theshold to 'th_dist - d'
+ node_out.th_dist_d = TH_DIST_ - node_out.d;
+ node_out.th_outlier_d = -node_out.d - TH_OUTLIER_;
+
+ return;
+ }
+
+ void modelPCAbasedTerrain(TriGridNode<PointType>& node_in) {
+ // Initailization
+ if (!ptCloud_nodewise_ground_.empty()) ptCloud_nodewise_ground_.clear();
+
+ // Tri Grid Initialization
+ // When to initialize the planar model, we don't have prior. so outlier is removed in heuristic parameter.
+ pcl::PointCloud<PointType> sort_ptCloud = node_in.ptCloud;
+
+ // sort in z-coordinate
+ sort(sort_ptCloud.points.begin(), sort_ptCloud.end(), point_z_cmp<PointType>);
+
+ // Set init seeds
+ extractInitialSeeds(sort_ptCloud, ptCloud_nodewise_ground_);
+
+ Eigen::MatrixXf points(sort_ptCloud.points.size(),3);
+ int j = 0;
+ for (auto& p:sort_ptCloud.points){
+ points.row(j++)<<p.x, p.y, p.z;
+ }
+ // Extract Ground
+ for (int i =0; i < NUM_ITER_; i++){
+ estimatePlanarModel(ptCloud_nodewise_ground_, node_in);
+ if(ptCloud_nodewise_ground_.size() < 3){
+
+ node_in.node_type = NONGROUND;
+ break;
+ }
+ ptCloud_nodewise_ground_.clear();
+ // threshold filter
+ Eigen::VectorXf result = points*node_in.normal;
+ for (int r = 0; r<result.rows(); r++){
+ if (i < NUM_ITER_-1){
+ if (result[r]<node_in.th_dist_d){
+ ptCloud_nodewise_ground_.push_back(sort_ptCloud.points[r]);
+ }
+ } else {
+ // Final interation
+ if (node_in.normal(2,0) < TH_NORMAL_ ){
+ node_in.node_type = NONGROUND;
+ } else {
+ node_in.node_type = GROUND;
+ }
+ }
+ }
+ }
+
+ return;
+ }
+
+ double calcNodeWeight(const TriGridNode<PointType>& node_in){
+ double weight = 0;
+
+ // weight = (node_in.singular_values[0]/node_in.singular_values[2] + node_in.singular_values[1]/node_in.singular_values[2])/(node_in.singular_values[0]/node_in.singular_values[1]);
+ weight = (node_in.singular_values[0] + node_in.singular_values[1])*node_in.singular_values[1]/(node_in.singular_values[0]*node_in.singular_values[2]+0.001);
+
+ return weight;
+ }
+
+ void modelNodeWiseTerrain(TriGridField<PointType>& tgf_in) {
+ // ROS_INFO("Node-wise Terrain Modeling...");
+
+ for (int r_i = 0; r_i < rows_; r_i++){
+ for (int c_i = 0; c_i < cols_; c_i++){
+ for (int s_i = 0; s_i < 4; s_i++){
+ if (tgf_in[r_i][c_i][s_i].is_curr_data){
+ if (tgf_in[r_i][c_i][s_i].ptCloud.size() < NUM_MIN_POINTS_){
+ tgf_in[r_i][c_i][s_i].node_type = UNKNOWN;
+ continue;
+ } else {
+ modelPCAbasedTerrain(tgf_in[r_i][c_i][s_i]);
+ if (tgf_in[r_i][c_i][s_i].node_type == GROUND){ tgf_in[r_i][c_i][s_i].weight = calcNodeWeight(tgf_in[r_i][c_i][s_i]); }
+ }
+ }
+ }
+ }
+ }
+
+ return;
+ };
+
+ void findDominantNode(const TriGridField<PointType>& tgf_in, TriGridIdx& node_idx_out) {
+ // Find the dominant node
+ ROS_INFO("Find the dominant node...");
+ TriGridIdx max_tri_idx;
+ TriGridIdx ego_idx;
+ ego_idx.row = (int)((0-tgf_min_x)/TGF_RESOLUTION_);
+ ego_idx.col = (int)((0-tgf_min_y)/TGF_RESOLUTION_);
+ ego_idx.tri = 0;
+
+ max_tri_idx = ego_idx;
+ for (int r_i = ego_idx.row - 2; r_i < ego_idx.row + 2; r_i++){
+ for (int c_i = ego_idx.col - 2; c_i < ego_idx.col + 2; c_i++){
+ for (int s_i = 0; s_i < 4; s_i++){
+ if (tgf_in[r_i][c_i][s_i].is_curr_data){
+ if (tgf_in[r_i][c_i][s_i].node_type == GROUND){
+ if (tgf_in[r_i][c_i][s_i].weight > tgf_in[max_tri_idx.row][max_tri_idx.row][max_tri_idx.tri].weight){
+ max_tri_idx.row = r_i;
+ max_tri_idx.col = c_i;
+ max_tri_idx.tri = s_i;
+ }
+ }
+ }
+ }
+ }
+ }
+ node_idx_out = max_tri_idx;
+ return;
+ };
+
+ void searchNeighborNodes(const TriGridIdx &cur_idx, std::vector<TriGridIdx> &neighbor_idxs) {
+ neighbor_idxs.clear();
+ neighbor_idxs.reserve(14);
+ int r_i = cur_idx.row;
+ int c_i = cur_idx.col;
+ int s_i = cur_idx.tri;
+
+ std::vector<TriGridIdx> tmp_neighbors;
+ tmp_neighbors.clear();
+ tmp_neighbors.reserve(14);
+
+ TriGridIdx neighbor_idx;
+ for (int s_i = 0; s_i < 4 ; s_i++){
+ if (s_i == cur_idx.tri) continue;
+
+ neighbor_idx = cur_idx;
+ neighbor_idx.tri = s_i;
+ tmp_neighbors.push_back(neighbor_idx);
+ }
+
+ switch (s_i) {
+ case 0:
+ tmp_neighbors.push_back({r_i+1, c_i+1, 2});
+ tmp_neighbors.push_back({r_i+1, c_i+1, 3});
+ tmp_neighbors.push_back({r_i+1, c_i , 1});
+ tmp_neighbors.push_back({r_i+1, c_i , 2});
+ tmp_neighbors.push_back({r_i+1, c_i , 3});
+ tmp_neighbors.push_back({r_i+1, c_i-1, 1});
+ tmp_neighbors.push_back({r_i+1, c_i-1, 2});
+ tmp_neighbors.push_back({r_i , c_i+1, 0});
+ tmp_neighbors.push_back({r_i , c_i+1, 3});
+ tmp_neighbors.push_back({r_i , c_i-1, 0});
+ tmp_neighbors.push_back({r_i , c_i-1, 1});
+ break;
+ case 1:
+ tmp_neighbors.push_back({r_i+1, c_i+1, 2});
+ tmp_neighbors.push_back({r_i+1, c_i+1, 3});
+ tmp_neighbors.push_back({r_i+1, c_i , 1});
+ tmp_neighbors.push_back({r_i+1, c_i , 2});
+ tmp_neighbors.push_back({r_i , c_i+1, 0});
+ tmp_neighbors.push_back({r_i , c_i+1, 2});
+ tmp_neighbors.push_back({r_i , c_i+1, 3});
+ tmp_neighbors.push_back({r_i-1, c_i+1, 0});
+ tmp_neighbors.push_back({r_i-1, c_i+1, 3});
+ tmp_neighbors.push_back({r_i-1, c_i+1, 0});
+ tmp_neighbors.push_back({r_i-1, c_i , 1});
+ break;
+ case 2:
+ tmp_neighbors.push_back({r_i , c_i+1, 2});
+ tmp_neighbors.push_back({r_i , c_i+1, 3});
+ tmp_neighbors.push_back({r_i , c_i-1, 1});
+ tmp_neighbors.push_back({r_i , c_i-1, 2});
+ tmp_neighbors.push_back({r_i-1, c_i+1, 0});
+ tmp_neighbors.push_back({r_i-1, c_i+1, 3});
+ tmp_neighbors.push_back({r_i-1, c_i , 0});
+ tmp_neighbors.push_back({r_i-1, c_i , 1});
+ tmp_neighbors.push_back({r_i-1, c_i , 3});
+ tmp_neighbors.push_back({r_i-1, c_i-1, 0});
+ tmp_neighbors.push_back({r_i-1, c_i-1, 1});
+ break;
+ case 3:
+ tmp_neighbors.push_back({r_i+1, c_i , 2});
+ tmp_neighbors.push_back({r_i+1, c_i , 3});
+ tmp_neighbors.push_back({r_i+1, c_i-1, 1});
+ tmp_neighbors.push_back({r_i+1, c_i-1, 2});
+ tmp_neighbors.push_back({r_i , c_i-1, 0});
+ tmp_neighbors.push_back({r_i , c_i-1, 1});
+ tmp_neighbors.push_back({r_i , c_i-1, 2});
+ tmp_neighbors.push_back({r_i-1, c_i , 0});
+ tmp_neighbors.push_back({r_i-1, c_i , 3});
+ tmp_neighbors.push_back({r_i-1, c_i-1, 0});
+ tmp_neighbors.push_back({r_i-1, c_i-1, 1});
+ break;
+ default:
+ break;
+ }
+
+ for (int n_i = 0 ; n_i < (int) tmp_neighbors.size() ; n_i++) {
+
+ if (tmp_neighbors[n_i].row >= rows_ || tmp_neighbors[n_i].row < 0) {
+ continue;
+ }
+
+ if (tmp_neighbors[n_i].col >= cols_ || tmp_neighbors[n_i].col < 0) {
+ continue;
+ }
+
+ neighbor_idxs.push_back(tmp_neighbors[n_i]);
+ }
+ }
+
+ void searchAdjacentNodes(const TriGridIdx &cur_idx, std::vector<TriGridIdx> &adjacent_idxs) {
+ adjacent_idxs.clear();
+ adjacent_idxs.reserve(3);
+ int r_i = cur_idx.row;
+ int c_i = cur_idx.col;
+ int s_i = cur_idx.tri;
+
+ std::vector<TriGridIdx> tmp_neighbors;
+ tmp_neighbors.clear();
+ tmp_neighbors.reserve(3);
+
+ TriGridIdx neighbor_idx;
+
+ switch (s_i) {
+ case 0:
+ tmp_neighbors.push_back({r_i+1, c_i, 2});
+ tmp_neighbors.push_back({r_i , c_i, 3});
+ tmp_neighbors.push_back({r_i , c_i, 1});
+
+ break;
+ case 1:
+ tmp_neighbors.push_back({r_i, c_i+1, 3});
+ tmp_neighbors.push_back({r_i, c_i , 0});
+ tmp_neighbors.push_back({r_i, c_i , 2});
+ break;
+ case 2:
+ tmp_neighbors.push_back({r_i-1, c_i, 0});
+ tmp_neighbors.push_back({r_i , c_i, 1});
+ tmp_neighbors.push_back({r_i , c_i, 3});
+ break;
+ case 3:
+ tmp_neighbors.push_back({r_i, c_i-1, 1});
+ tmp_neighbors.push_back({r_i, c_i , 2});
+ tmp_neighbors.push_back({r_i, c_i , 0});
+ break;
+ default:
+ break;
+ }
+
+ for (int n_i = 0 ; n_i < (int) tmp_neighbors.size() ; n_i++) {
+
+ if (tmp_neighbors[n_i].row >= rows_ || tmp_neighbors[n_i].row < 0) {
+ continue;
+ }
+
+ if (tmp_neighbors[n_i].col >= cols_ || tmp_neighbors[n_i].col < 0) {
+ continue;
+ }
+
+ adjacent_idxs.push_back(tmp_neighbors[n_i]);
+ }
+ }
+
+ bool LocalConvecityConcavity(const TriGridField<PointType> &tgf, const TriGridIdx &cur_node_idx, const TriGridIdx &neighbor_idx,
+ double & thr_local_normal, double & thr_local_dist) {
+ TriGridNode<PointType> current_node = tgf[cur_node_idx.row][cur_node_idx.col][cur_node_idx.tri];
+ TriGridNode<PointType> neighbor_node = tgf[neighbor_idx.row][neighbor_idx.col][neighbor_idx.tri];
+
+ Eigen::Vector3f normal_src = current_node.normal;
+ Eigen::Vector3f normal_tgt = neighbor_node.normal;
+ Eigen::Vector3f meanPt_diff_s2t = neighbor_node.mean_pt - current_node.mean_pt;
+
+ double diff_norm = meanPt_diff_s2t.norm();
+ double dist_s2t = normal_src.dot(meanPt_diff_s2t);
+ double dist_t2s = normal_tgt.dot(-meanPt_diff_s2t);
+
+ double normal_similarity = normal_src.dot(normal_tgt);
+ double TH_NORMAL_cos_similarity = sin(diff_norm*thr_local_normal);
+ if ((normal_similarity < (1-TH_NORMAL_cos_similarity))) {
+ return false;
+ }
+
+ double TH_DIST_to_planar = diff_norm*sin(thr_local_dist);
+ if ( (abs(dist_s2t) > TH_DIST_to_planar || abs(dist_t2s) > TH_DIST_to_planar) ) {
+ return false;
+ }
+
+ return true;
+ }
+
+ void BreadthFirstTraversableGraphSearch(TriGridField<PointType>& tgf_in) {
+
+ // Find the dominant node
+ std::queue<TriGridIdx> searching_idx_queue;
+ TriGridIdx dominant_node_idx;
+ findDominantNode(tgf_in, dominant_node_idx);
+ tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].is_visited = true;
+ tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].depth = 0;
+ tgf_in[dominant_node_idx.row][dominant_node_idx.col][dominant_node_idx.tri].node_type = GROUND;
+
+ searching_idx_queue.push(dominant_node_idx);
+
+ double max_planar_height = 0;
+ trigrid_edges_.clear();
+ trigrid_edges_.reserve(rows_*cols_*4);
+ TriGridEdge cur_edge;
+ TriGridIdx current_node_idx;
+ while (!searching_idx_queue.empty()){
+ // set current node
+ current_node_idx = searching_idx_queue.front();
+ searching_idx_queue.pop();
+
+ // search the neighbor nodes
+ std::vector<TriGridIdx> neighbor_idxs;
+ searchNeighborNodes(current_node_idx, neighbor_idxs);
+
+ // set the traversable edges
+ for (int i = 0; i < (int) neighbor_idxs.size(); i++){
+ // if the neighbor node is traversable, add it to the queue
+
+ TriGridIdx n_i = neighbor_idxs[i];
+
+
+ if (tgf_in[n_i.row][n_i.col][n_i.tri].depth >=0){
+ continue;
+ }
+
+ if (tgf_in[n_i.row][n_i.col][n_i.tri].is_visited) {
+ if (!tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck){
+ continue;
+ } else {
+ if (tgf_in[n_i.row][n_i.col][n_i.tri].check_life <= 0){
+ continue;
+ }
+ }
+ continue;
+ } else {
+ if (tgf_in[n_i.row][n_i.col][n_i.tri].node_type != GROUND) {
+ continue;
+ }
+ }
+
+ tgf_in[n_i.row][n_i.col][n_i.tri].is_visited =true;
+
+ if (!LocalConvecityConcavity(tgf_in, current_node_idx, n_i, TH_LCC_NORMAL_SIMILARITY_, TH_LCC_PLANAR_MODEL_DIST_)) {
+ tgf_in[n_i.row][n_i.col][n_i.tri].is_rejection = true;
+ tgf_in[n_i.row][n_i.col][n_i.tri].node_type = NONGROUND;
+
+ if(tgf_in[n_i.row][n_i.col][n_i.tri].check_life > 0) {
+ tgf_in[n_i.row][n_i.col][n_i.tri].check_life -=1;
+ tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck = true;
+ } else {
+ tgf_in[n_i.row][n_i.col][n_i.tri].need_recheck = false;
+ }
+ continue;
+ }
+
+ if (max_planar_height < tgf_in[n_i.row][n_i.col][n_i.tri].mean_pt[2]) max_planar_height = tgf_in[n_i.row][n_i.col][n_i.tri].mean_pt[2];
+
+ tgf_in[n_i.row][n_i.col][n_i.tri].node_type = GROUND;
+ tgf_in[n_i.row][n_i.col][n_i.tri].is_rejection = false;
+ tgf_in[n_i.row][n_i.col][n_i.tri].depth = tgf_in[current_node_idx.row][current_node_idx.col][current_node_idx.tri].depth + 1;
+
+ if (VIZ_MDOE_){
+ cur_edge.Pair.first = current_node_idx;
+ cur_edge.Pair.second = n_i;
+ cur_edge.is_traversable = true;
+ trigrid_edges_.push_back(cur_edge);
+ }
+
+ searching_idx_queue.push(n_i);
+ }
+
+ if (searching_idx_queue.empty()){
+ // set the new dominant node
+ for (int r_i = 0; r_i < rows_; r_i++) {
+ for (int c_i = 0; c_i < cols_; c_i++) {
+ for (int s_i = 0; s_i < (int) tgf_in[r_i][c_i].size() ; s_i++){
+ if (tgf_in[r_i][c_i][s_i].is_visited) { continue; }
+
+ if (tgf_in[r_i][c_i][s_i].node_type != GROUND ) { continue; }
+
+ // if (tgf_in[r_i][c_i][s_i].mean_pt[2] >= max_planar_height+1) { continue; }
+
+ if (tgf_in[r_i][c_i][s_i].depth >= 0) { continue; }
+
+ // if (tgf_in[r_i][c_i][s_i].weight > 5*TH_WEIGHT_){
+ tgf_in[r_i][c_i][s_i].depth = 0;
+ tgf_in[r_i][c_i][s_i].is_visited = true;
+
+ TriGridIdx new_dominant_idx = {r_i, c_i, s_i};
+ searching_idx_queue.push(new_dominant_idx);
+ // }
+ }
+ }
+ }
+ }
+ }
+ return;
+ };
+
+
+ double getCornerWeight(const TriGridNode<PointType>& node_in, const pcl::PointXYZ &tgt_corner){
+ double xy_dist = sqrt( (node_in.mean_pt[0]-tgt_corner.x)*(node_in.mean_pt[0]-tgt_corner.x)+(node_in.mean_pt[1]-tgt_corner.y)*(node_in.mean_pt[1]-tgt_corner.y) );
+ return (node_in.weight/xy_dist);
+ }
+
+ void setTGFCornersCenters(const TriGridField<PointType>& tgf_in,
+ std::vector<std::vector<TriGridCorner>>& trigrid_corners_out,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers_out) {
+ pcl::PointXYZ corner_TL, corner_BL, corner_BR, corner_TR, corner_C;
+
+ for (int r_i = 0; r_i<rows_; r_i++){
+ for (int c_i = 0; c_i<cols_; c_i++){
+ corner_TL.x = trigrid_corners_out[r_i+1][c_i+1].x; corner_TL.y = trigrid_corners_out[r_i+1][c_i+1].y; // LT
+ corner_BL.x = trigrid_corners_out[r_i][c_i+1].x; corner_BL.y = trigrid_corners_out[r_i][c_i+1].y; // LL
+ corner_BR.x = trigrid_corners_out[r_i][c_i].x; corner_BR.y = trigrid_corners_out[r_i][c_i].y; // RL
+ corner_TR.x = trigrid_corners_out[r_i+1][c_i].x; corner_TR.y = trigrid_corners_out[r_i+1][c_i].y; // RT
+ corner_C.x = trigrid_centers_out[r_i][c_i].x; corner_C.y = trigrid_centers_out[r_i][c_i].y; // Center
+
+ for (int s_i = 0; s_i< (int) tgf_in[r_i][c_i].size();s_i++){
+ if (tgf_in[r_i][c_i][s_i].node_type != GROUND) { continue; }
+ if (tgf_in[r_i][c_i][s_i].is_rejection) { continue; }
+ if (tgf_in[r_i][c_i][s_i].depth == -1) { continue; }
+
+ switch(s_i){
+ case 0: // upper Tri-grid bin
+ // RT / LT / C
+ trigrid_corners_out[r_i+1][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_TR.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_TR.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i+1][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_TR));
+
+ trigrid_corners_out[r_i+1][c_i+1].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_TL.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_TL.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i+1][c_i+1].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_TL));
+
+ trigrid_centers_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_C.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_C.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_centers_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_C));
+
+ break;
+ case 1: // left Tri-grid bin
+ // LT / LL / C
+ trigrid_corners_out[r_i+1][c_i+1].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_TL.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_TL.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i+1][c_i+1].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_TL));
+
+ trigrid_corners_out[r_i][c_i+1].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_BL.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_BL.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i][c_i+1].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_BL));
+
+ trigrid_centers_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_C.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_C.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_centers_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_C));
+
+ break;
+ case 2: // lower Tri-grid bin
+ // LL / RL / C
+ trigrid_corners_out[r_i][c_i+1].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_BL.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_BL.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i][c_i+1].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_BL));
+
+ trigrid_corners_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_BR.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_BR.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_BR));
+
+ trigrid_centers_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_C.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_C.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_centers_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_C));
+
+ break;
+ case 3: // right Tri-grid bin
+ // RL / RT / C
+ trigrid_corners_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_BR.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_BR.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_BR));
+
+ trigrid_corners_out[r_i+1][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_TR.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_TR.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_corners_out[r_i+1][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_TR));
+
+ trigrid_centers_out[r_i][c_i].zs.push_back( (-tgf_in[r_i][c_i][s_i].normal(0,0)*corner_C.x - tgf_in[r_i][c_i][s_i].normal(1,0)*corner_C.y-tgf_in[r_i][c_i][s_i].d)/tgf_in[r_i][c_i][s_i].normal(2,0) );
+ trigrid_centers_out[r_i][c_i].weights.push_back(getCornerWeight(tgf_in[r_i][c_i][s_i],corner_C));
+
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }
+ return;
+ };
+
+ TriGridCorner getMeanCorner(const TriGridCorner &corners_in){
+ // get the mean of the corners
+
+ TriGridCorner corners_out;
+ corners_out.x = corners_in.x;
+ corners_out.y = corners_in.y;
+ corners_out.zs.clear();
+ corners_out.weights.clear();
+
+ double weighted_sum_z = 0.0;
+ double sum_w = 0.0;
+ for (int i = 0; i < (int) corners_in.zs.size(); i++){
+ weighted_sum_z += corners_in.zs[i]*corners_in.weights[i];
+ sum_w += corners_in.weights[i];
+ }
+
+ corners_out.zs.push_back(weighted_sum_z/sum_w);
+ corners_out.weights.push_back(sum_w);
+
+ return corners_out;
+ }
+
+ void updateTGFCornersCenters(std::vector<std::vector<TriGridCorner>>& trigrid_corners_out,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers_out) {
+
+ // update corners
+ TriGridCorner updated_corner = empty_trigrid_corner_;
+ for (int r_i = 0; r_i < rows_ +1; r_i++) {
+ for (int c_i = 0; c_i < cols_ +1; c_i++) {
+ if (trigrid_corners_out[r_i][c_i].zs.size() > 0 && trigrid_corners_out[r_i][c_i].weights.size() > 0) {
+ updated_corner = getMeanCorner(trigrid_corners_out[r_i][c_i]);
+ trigrid_corners_out[r_i][c_i] = updated_corner;
+ } else {
+ trigrid_corners_out[r_i][c_i].zs.clear();
+ trigrid_corners_out[r_i][c_i].weights.clear();
+ }
+ }
+ }
+
+ // update centers
+ TriGridCorner updated_center = empty_trigrid_center_;
+ for (int r_i = 0; r_i < rows_; r_i++) {
+ for (int c_i = 0; c_i < cols_; c_i++) {
+ if (trigrid_centers_out[r_i][c_i].zs.size() > 0 && trigrid_centers_out[r_i][c_i].weights.size() > 0) {
+ updated_center = getMeanCorner(trigrid_centers_out[r_i][c_i]);
+ trigrid_centers_out[r_i][c_i] = updated_center;
+ // trigrid_centers_out[r_i][c_i].z = get_mean(trigrid_centers_out[r_i][c_i].zs,trigrid_centers_out[r_i][c_i].weights);
+ } else {
+ trigrid_centers_out[r_i][c_i].zs.clear();
+ trigrid_centers_out[r_i][c_i].weights.clear();
+ }
+ }
+ }
+
+ return;
+ };
+
+ Eigen::Vector3f convertCornerToEigen(TriGridCorner &corner_in) {
+ Eigen::Vector3f corner_out;
+ if (corner_in.zs.size() != corner_in.weights.size()){
+ ROS_WARN("ERROR in corners");
+ }
+ corner_out[0] = corner_in.x;
+ corner_out[1] = corner_in.y;
+ corner_out[2] = corner_in.zs[0];
+ return corner_out;
+ };
+
+ void revertTraversableNodes(std::vector<std::vector<TriGridCorner>>& trigrid_corners_in,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers_in,
+ TriGridField<PointType>& tgf_out) {
+ Eigen::Vector3f refined_corner_1, refined_corner_2, refined_center;
+ for (int r_i = 0; r_i < rows_; r_i++) {
+ for (int c_i = 0; c_i < cols_; c_i++) {
+ for (int s_i = 0; s_i < (int) tgf_out[r_i][c_i].size(); s_i++) {
+ // set the corners for the current trigrid node
+ switch (s_i)
+ {
+ case 0:
+ if ( trigrid_corners_in[r_i+1][c_i].zs.size()==0 || trigrid_corners_in[r_i+1][c_i+1].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){
+ if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){
+ tgf_out[r_i][c_i][s_i].node_type = UNKNOWN;
+ }
+ continue;
+ }
+ refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i]);
+ refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i+1]);
+ refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]);
+ break;
+ case 1:
+ if ( trigrid_corners_in[r_i+1][c_i+1].zs.size()==0 || trigrid_corners_in[r_i][c_i+1].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){
+ if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){
+ tgf_out[r_i][c_i][s_i].node_type = UNKNOWN;
+ }
+ continue;
+ }
+ refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i+1]);
+ refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i][c_i+1]);
+ refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]);
+ break;
+ case 2:
+ if ( trigrid_corners_in[r_i][c_i+1].zs.size()==0 || trigrid_corners_in[r_i][c_i].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){
+ if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){
+ tgf_out[r_i][c_i][s_i].node_type = UNKNOWN;
+ }
+ continue;
+ }
+ refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i][c_i+1]);
+ refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i][c_i]);
+ refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]);
+ break;
+ case 3:
+ if ( trigrid_corners_in[r_i][c_i].zs.size()==0 || trigrid_corners_in[r_i+1][c_i].zs.size()==0 || trigrid_centers_in[r_i][c_i].zs.size()==0 ){
+ if (tgf_out[r_i][c_i][s_i].node_type != NONGROUND){
+ tgf_out[r_i][c_i][s_i].node_type = UNKNOWN;
+ }
+ continue;
+ }
+ refined_corner_1 = convertCornerToEigen(trigrid_corners_in[r_i][c_i]);
+ refined_corner_2 = convertCornerToEigen(trigrid_corners_in[r_i+1][c_i]);
+ refined_center = convertCornerToEigen(trigrid_centers_in[r_i][c_i]);
+ break;
+ default:
+ ROS_ERROR("WRONG tri-grid indexing");
+ break;
+ }
+
+ // calculate the refined planar model in the node
+ Eigen::Vector3f udpated_normal = (refined_corner_1-refined_center).cross(refined_corner_2-refined_center);
+ udpated_normal /= udpated_normal.norm();
+ // if (udpated_normal[2] < 0){
+ // std::cout << "Origin normal: " << tgf_out[r_i][c_i][s_i].normal << std::endl;
+ // std::cout << "Update normal: " << udpated_normal << std::endl;
+ // }
+ if (udpated_normal(2,0) < TH_NORMAL_ ){ // non-planar
+ tgf_out[r_i][c_i][s_i].normal = udpated_normal;
+ tgf_out[r_i][c_i][s_i].node_type = NONGROUND;
+ } else {
+ // planar
+ Eigen::Vector3f updated_mean_pt;
+ updated_mean_pt[0] = (refined_corner_1[0] + refined_corner_2[0] + refined_center[0])/3;
+ updated_mean_pt[1] = (refined_corner_1[1] + refined_corner_2[1] + refined_center[1])/3;
+ updated_mean_pt[2] = (refined_corner_1[2] + refined_corner_2[2] + refined_center[2])/3;
+
+ tgf_out[r_i][c_i][s_i].normal = udpated_normal;
+ tgf_out[r_i][c_i][s_i].mean_pt = updated_mean_pt;
+ tgf_out[r_i][c_i][s_i].d = -(udpated_normal.dot(updated_mean_pt));
+ tgf_out[r_i][c_i][s_i].th_dist_d = TH_DIST_ - tgf_out[r_i][c_i][s_i].d;
+ tgf_out[r_i][c_i][s_i].th_outlier_d = -TH_OUTLIER_ - tgf_out[r_i][c_i][s_i].d;
+
+ tgf_out[r_i][c_i][s_i].node_type = GROUND;
+ }
+ }
+ }
+ }
+
+ return;
+ };
+
+ void fitTGFWiseTraversableTerrainModel(TriGridField<PointType>& tgf,
+ std::vector<std::vector<TriGridCorner>>& trigrid_corners,
+ std::vector<std::vector<TriGridCorner>>& trigrid_centers) {
+
+ updateTGFCornersCenters(trigrid_corners, trigrid_centers);
+
+ revertTraversableNodes(trigrid_corners, trigrid_centers, tgf);
+
+ return;
+ };
+
+ void segmentNodeGround(const TriGridNode<PointType>& node_in,
+ pcl::PointCloud<PointType>& node_ground_out,
+ pcl::PointCloud<PointType>& node_nonground_out,
+ pcl::PointCloud<PointType>& node_obstacle_out,
+ pcl::PointCloud<PointType>& node_outlier_out) {
+ node_ground_out.clear();
+ node_nonground_out.clear();
+ node_obstacle_out.clear();
+ node_outlier_out.clear();
+
+ // segment ground
+ Eigen::MatrixXf points(node_in.ptCloud.points.size(),3);
+ int j = 0;
+ for (auto& p:node_in.ptCloud.points){
+ points.row(j++)<<p.x, p.y, p.z;
+ }
+
+ Eigen::VectorXf result = points*node_in.normal;
+ for (int r = 0; r<result.rows(); r++){
+ if (result[r]<node_in.th_dist_d){
+ if (result[r]<node_in.th_outlier_d){
+ node_outlier_out.push_back(node_in.ptCloud.points[r]);
+ } else {
+ node_ground_out.push_back(node_in.ptCloud.points[r]);
+ }
+ } else {
+ node_nonground_out.push_back(node_in.ptCloud.points[r]);
+ if (result[r]<TH_OBSTACLE_HEIGHT_ - node_in.d){
+ node_obstacle_out.push_back(node_in.ptCloud.points[r]);
+ node_obstacle_out.points.back().intensity = result[r] + node_in.d;
+ }
+ }
+ }
+
+ return;
+ }
+
+ void segmentTGFGround(const TriGridField<PointType>& tgf_in,
+ pcl::PointCloud<PointType>& ground_cloud_out,
+ pcl::PointCloud<PointType>& nonground_cloud_out,
+ pcl::PointCloud<PointType>& obstacle_cloud_out,
+ pcl::PointCloud<PointType>& outlier_cloud_out) {
+ ground_cloud_out.clear();
+ nonground_cloud_out.clear();
+ obstacle_cloud_out.clear();
+
+ for (int r_i = 0; r_i < rows_; r_i++){
+ for (int c_i = 0; c_i < cols_; c_i++){
+ for (int s_i = 0; s_i < tgf_in[r_i][c_i].size(); s_i++) {
+ if (!tgf_in[r_i][c_i][s_i].is_curr_data) {
+ continue;
+ }
+ if (tgf_in[r_i][c_i][s_i].node_type == GROUND) {
+ segmentNodeGround(tgf_in[r_i][c_i][s_i], ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_);
+ } else {
+ ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ }
+ ground_cloud_out += ptCloud_nodewise_ground_;
+ nonground_cloud_out += ptCloud_nodewise_nonground_;
+ outlier_cloud_out += ptCloud_nodewise_outliers_;
+ obstacle_cloud_out += ptCloud_nodewise_obstacle_;
+ }
+ }
+ }
+
+ return;
+ };
+
+ void segmentTGFGround_developing(const TriGridField<PointType>& tgf_in,
+ pcl::PointCloud<PointType>& ground_cloud_out,
+ pcl::PointCloud<PointType>& nonground_cloud_out,
+ pcl::PointCloud<PointType>& obstacle_cloud_out,
+ pcl::PointCloud<PointType>& outlier_cloud_out) {
+ ground_cloud_out.clear();
+ nonground_cloud_out.clear();
+ obstacle_cloud_out.clear();
+ TriGridIdx curr_tgf_idx;
+ std::vector<TriGridIdx> adj_idx_vec;
+ pcl::PointCloud<PointType> outlier_tmp;
+ outlier_tmp.clear();
+ outlier_tmp.reserve(NODEWISE_PTCLOUDSIZE);
+ for (int r_i = 0; r_i < rows_; r_i++){
+ for (int c_i = 0; c_i < cols_; c_i++){
+ for (int s_i = 0; s_i < (int) tgf_in[r_i][c_i].size(); s_i++) {
+ if (!tgf_in[r_i][c_i][s_i].is_curr_data) {
+ continue;
+ }
+ if (tgf_in[r_i][c_i][s_i].node_type == UNKNOWN){
+ continue;
+ }
+ if (tgf_in[r_i][c_i][s_i].node_type == GROUND) {
+ segmentNodeGround(tgf_in[r_i][c_i][s_i], ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_);
+ } else {
+ curr_tgf_idx.row = r_i;
+ curr_tgf_idx.col = c_i;
+ curr_tgf_idx.tri = s_i;
+
+ searchAdjacentNodes(curr_tgf_idx, adj_idx_vec);
+ if (adj_idx_vec.empty()){
+ ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ } else {
+ TriGridIdx highest_adj_tri_idx;
+ double highest_weight = 0;
+ bool is_adjacent = false;
+ for (int adj_i = 0; adj_i<adj_idx_vec.size() ; adj_i++){
+ if (getTriGridNode(adj_idx_vec[adj_i]).weight < TH_WEIGHT_) {
+ continue;
+ }
+ is_adjacent = true;
+ if (getTriGridNode(adj_idx_vec[adj_i]).weight > highest_weight) {
+ highest_weight = getTriGridNode(adj_idx_vec[adj_i]).weight;
+ highest_adj_tri_idx = adj_idx_vec[adj_i];
+ }
+ }
+
+ if (is_adjacent){
+ TriGridNode<PointType> tmp_node = getTriGridNode(highest_adj_tri_idx);
+ tmp_node.ptCloud = getTriGridNode(curr_tgf_idx).ptCloud;
+ segmentNodeGround(tmp_node, ptCloud_nodewise_ground_, ptCloud_nodewise_nonground_, ptCloud_nodewise_obstacle_, ptCloud_nodewise_outliers_);
+ } else {
+ ptCloud_nodewise_nonground_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ ptCloud_nodewise_obstacle_ = tgf_in[r_i][c_i][s_i].ptCloud;
+ }
+ }
+ }
+ ground_cloud_out += ptCloud_nodewise_ground_;
+ nonground_cloud_out += ptCloud_nodewise_nonground_;
+ obstacle_cloud_out += ptCloud_nodewise_obstacle_;
+ outlier_cloud_out += ptCloud_nodewise_outliers_;
+ }
+ }
+ }
+
+ return;
+ };
+
+ // functions for visualization
+
+ geometry_msgs::PolygonStamped setPlanarModel (const TriGridNode<PointType>& node_in, const TriGridIdx& node_idx) {
+ geometry_msgs::PolygonStamped polygon_out;
+ polygon_out.header = msg_header_;
+ geometry_msgs::Point32 corner_0, corner_1, corner_2;
+ int r_i = node_idx.row;
+ int c_i = node_idx.col;
+ int s_i = node_idx.tri;
+ if (node_in.node_type == GROUND){
+ switch (s_i){
+ case 0:
+ //topx lowy & topx topy
+ corner_1.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = (-node_in.normal(0,0)*corner_1.x - node_in.normal(1,0)*corner_1.y-node_in.d)/node_in.normal(2,0);
+
+ corner_2.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = (-node_in.normal(0,0)*corner_2.x - node_in.normal(1,0)*corner_2.y-node_in.d)/node_in.normal(2,0);
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = (-node_in.normal(0,0)*corner_0.x - node_in.normal(1,0)*corner_0.y-node_in.d)/node_in.normal(2,0);
+ break;
+ case 1:
+ //topx topy & lowx topy
+ corner_1.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = (-node_in.normal(0,0)*corner_1.x - node_in.normal(1,0)*corner_1.y-node_in.d)/node_in.normal(2,0);
+
+ corner_2.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = (-node_in.normal(0,0)*corner_2.x - node_in.normal(1,0)*corner_2.y-node_in.d)/node_in.normal(2,0);
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = (-node_in.normal(0,0)*corner_0.x - node_in.normal(1,0)*corner_0.y-node_in.d)/node_in.normal(2,0);
+ break;
+ case 2:
+ //lowx topy & lowx lowy
+ corner_1.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = (-node_in.normal(0,0)*corner_1.x - node_in.normal(1,0)*corner_1.y-node_in.d)/node_in.normal(2,0);
+
+ corner_2.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = c_i*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = (-node_in.normal(0,0)*corner_2.x - node_in.normal(1,0)*corner_2.y-node_in.d)/node_in.normal(2,0);
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = (-node_in.normal(0,0)*corner_0.x - node_in.normal(1,0)*corner_0.y-node_in.d)/node_in.normal(2,0);
+ break;
+ case 3:
+ //lowx lowy & topx lowy
+ corner_1.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = (-node_in.normal(0,0)*corner_1.x - node_in.normal(1,0)*corner_1.y-node_in.d)/node_in.normal(2,0);
+
+ corner_2.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = (-node_in.normal(0,0)*corner_2.x - node_in.normal(1,0)*corner_2.y-node_in.d)/node_in.normal(2,0);
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = (-node_in.normal(0,0)*corner_0.x - node_in.normal(1,0)*corner_0.y-node_in.d)/node_in.normal(2,0);
+ break;
+ default:
+ break;
+ }
+ } else {
+ switch (s_i){
+ case 0:
+ //topx lowy & topx topy
+ corner_1.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = -2.0;
+
+ corner_2.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = -2.0;
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = -2.0;
+ break;
+ case 1:
+ //topx topy & lowx topy
+ corner_1.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = -2.0;
+
+ corner_2.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = -2.0;
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = -2.0;
+ break;
+ case 2:
+ //lowx topy & lowx lowy
+ corner_1.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i+1)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = -2.0;
+
+ corner_2.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = c_i*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = -2.0;
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = -2.0;
+ break;
+ case 3:
+ //lowx lowy & topx lowy
+ corner_1.x = (r_i)*TGF_RESOLUTION_+tgf_min_x; corner_1.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_1.z = -2.0;
+
+ corner_2.x = (r_i+1)*TGF_RESOLUTION_+tgf_min_x; corner_2.y = (c_i)*TGF_RESOLUTION_+tgf_min_y;
+ corner_2.z = -2.0;
+
+ corner_0.x = (r_i+0.5)*TGF_RESOLUTION_+tgf_min_x; corner_0.y = (c_i+0.5)*TGF_RESOLUTION_+tgf_min_y;
+ corner_0.z = -2.0;
+ break;
+ default:
+ std::cout << "the error in sub-idx" << std::endl;
+ break;
+ }
+ }
+
+ polygon_out.polygon.points.reserve(3);
+ polygon_out.polygon.points.push_back(corner_0);
+ polygon_out.polygon.points.push_back(corner_2);
+ polygon_out.polygon.points.push_back(corner_1);
+ return polygon_out;
+ }
+
+ void publishTriGridFieldGraph() {
+ viz_trigrid_polygons_.header = msg_header_;
+ viz_trigrid_polygons_.polygons.clear();
+ viz_trigrid_polygons_.likelihood.clear();
+
+ // visualize the graph: nodes
+ for (int r_i = 0; r_i < rows_; r_i++){
+ for (int c_i = 0; c_i < cols_; c_i++){
+ for (int s_i = 0; s_i < (int) trigrid_field_[r_i][c_i].size(); s_i++){
+ TriGridIdx curr_idx = {r_i, c_i, s_i};
+ if (trigrid_field_[r_i][c_i][s_i].node_type == GROUND) {
+ viz_trigrid_polygons_.polygons.push_back(setPlanarModel(trigrid_field_[r_i][c_i][s_i], curr_idx));
+ // if (trigrid_field_[r_i][c_i][s_i].normal[2] < 0.99) {
+ // viz_trigrid_polygons_.likelihood.push_back(0.25);
+ // } else {
+ viz_trigrid_polygons_.likelihood.push_back(0.0);
+ // }
+
+ } else if (trigrid_field_[r_i][c_i][s_i].node_type == NONGROUND) {
+ // continue;
+ viz_trigrid_polygons_.polygons.push_back(setPlanarModel(trigrid_field_[r_i][c_i][s_i], curr_idx));
+ viz_trigrid_polygons_.likelihood.push_back(1.0);
+
+ } else if (trigrid_field_[r_i][c_i][s_i].node_type == UNKNOWN) {
+ // continue;
+ if (trigrid_field_[r_i][c_i][s_i].is_curr_data) {
+ viz_trigrid_polygons_.polygons.push_back(setPlanarModel(trigrid_field_[r_i][c_i][s_i], curr_idx));
+ viz_trigrid_polygons_.likelihood.push_back(0.5);
+ }
+ } else {
+ ROS_WARN("Unknown Node Type");
+ }
+ }
+ }
+ }
+
+ //visualize the graph: edges
+ viz_trigrid_edges_.header = msg_header_;
+ viz_trigrid_edges_.points.clear();
+ geometry_msgs::Point src_pt;
+ geometry_msgs::Point tgt_pt;
+ for (int e_i = 0; e_i < (int) trigrid_edges_.size(); e_i++){
+ if (trigrid_edges_[e_i].is_traversable){
+ viz_trigrid_edges_.color.a = 1.0;
+ viz_trigrid_edges_.color.r = 1.0;
+ viz_trigrid_edges_.color.g = 1.0;
+ viz_trigrid_edges_.color.b = 0.0;
+ } else {
+ viz_trigrid_edges_.color.a = 0.1;
+ viz_trigrid_edges_.color.r = 1.0;
+ viz_trigrid_edges_.color.g = 1.0;
+ viz_trigrid_edges_.color.b = 1.0;
+ }
+
+ src_pt.x = trigrid_field_[trigrid_edges_[e_i].Pair.first.row][trigrid_edges_[e_i].Pair.first.col][trigrid_edges_[e_i].Pair.first.tri].mean_pt[0];
+ src_pt.y = trigrid_field_[trigrid_edges_[e_i].Pair.first.row][trigrid_edges_[e_i].Pair.first.col][trigrid_edges_[e_i].Pair.first.tri].mean_pt[1];
+ src_pt.z = trigrid_field_[trigrid_edges_[e_i].Pair.first.row][trigrid_edges_[e_i].Pair.first.col][trigrid_edges_[e_i].Pair.first.tri].mean_pt[2];
+
+ tgt_pt.x = trigrid_field_[trigrid_edges_[e_i].Pair.second.row][trigrid_edges_[e_i].Pair.second.col][trigrid_edges_[e_i].Pair.second.tri].mean_pt[0];
+ tgt_pt.y = trigrid_field_[trigrid_edges_[e_i].Pair.second.row][trigrid_edges_[e_i].Pair.second.col][trigrid_edges_[e_i].Pair.second.tri].mean_pt[1];
+ tgt_pt.z = trigrid_field_[trigrid_edges_[e_i].Pair.second.row][trigrid_edges_[e_i].Pair.second.col][trigrid_edges_[e_i].Pair.second.tri].mean_pt[2];
+
+ viz_trigrid_edges_.points.push_back(src_pt);
+ viz_trigrid_edges_.points.push_back(tgt_pt);
+ }
+
+ // pub_trigrid_nodes_.publish(viz_trigrid_polygons_);
+ // pub_trigrid_edges_.publish(viz_trigrid_edges_);
+ return;
+ };
+
+ // void publishTriGridCorners() {
+ // viz_trigrid_corners_.header = cloud_header_;
+ // viz_trigrid_corners_.points.clear();
+
+ // TriGridCorner curr_corner;
+ // pcl::PointXYZ corner_pt;
+ // // for corners
+ // for (int r_i = 0; r_i < (int) trigrid_corners_.size(); r_i++){
+ // for (int c_i = 0; c_i < (int) trigrid_corners_[0].size(); c_i++){
+ // curr_corner = trigrid_corners_[r_i][c_i];
+ // if (curr_corner.zs.size() != curr_corner.weights.size()){
+ // ROS_WARN("ERROR in corners");
+ // }
+ // for (int i = 0; i < (int) curr_corner.zs.size(); i++){
+ // corner_pt.x = curr_corner.x;
+ // corner_pt.y = curr_corner.y;
+ // corner_pt.z = curr_corner.zs[i];
+ // viz_trigrid_corners_.points.push_back(corner_pt);
+ // }
+ // }
+ // }
+
+ // // for centers
+ // for (int r_i = 0; r_i < (int) trigrid_centers_.size(); r_i++){
+ // for (int c_i = 0; c_i < (int) trigrid_centers_[0].size(); c_i++){
+ // curr_corner = trigrid_centers_[r_i][c_i];
+ // if (curr_corner.zs.size() != curr_corner.weights.size()){
+ // ROS_WARN("ERROR in corners");
+ // }
+ // for (int i = 0; i < (int) curr_corner.zs.size(); i++){
+ // corner_pt.x = curr_corner.x;
+ // corner_pt.y = curr_corner.y;
+ // corner_pt.z = curr_corner.zs[i];
+ // viz_trigrid_corners_.points.push_back(corner_pt);
+ // }
+ // }
+ // }
+
+ // pub_trigrid_corners_.publish(viz_trigrid_corners_);
+ // return;
+ // };
+
+ // sensor_msgs::PointCloud2 convertCloudToRosMsg(pcl::PointCloud<PointType>& cloud, std::string &frame_id) {
+ // sensor_msgs::PointCloud2 cloud_msg;
+ // pcl::toROSMsg(cloud, cloud_msg);
+ // cloud_msg.header.frame_id = frame_id;
+ // return cloud_msg;
+ // };
+
+ // void publishPointClouds(){
+ // ptCloud_tgfwise_ground_.header = cloud_header_;
+ // pub_tgseg_ground_cloud.publish(convertCloudToRosMsg(ptCloud_tgfwise_ground_, cloud_header_.frame_id));
+
+ // ptCloud_tgfwise_nonground_.header = cloud_header_;
+ // pub_tgseg_nonground_cloud.publish(convertCloudToRosMsg(ptCloud_tgfwise_nonground_, cloud_header_.frame_id));
+
+ // ptCloud_tgfwise_outliers_.header = cloud_header_;
+ // pub_tgseg_outliers_cloud.publish(convertCloudToRosMsg(ptCloud_tgfwise_outliers_, cloud_header_.frame_id));
+ // }
+ };
+}
+#endif
diff --git a/launch/iri_ground_segmentation.launch b/launch/iri_ground_segmentation.launch
index 1e64bbdacc744af860cb1f5f756bcf6258c04d36..eea1a684fbdaff7c5917ecbf71b1102f8b9a1a45 100644
--- a/launch/iri_ground_segmentation.launch
+++ b/launch/iri_ground_segmentation.launch
@@ -1,5 +1,7 @@
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find iri_ground_segmentation)/params/iri_ground_segmentation.yaml" />
- <node pkg="iri_ground_segmentation" type="iri_ground_segmentation" name="iri_ground_segmentation" output="screen"/>
+ <remap from="/lidar_points_extended_structure" to="/xyzi_to_extended_structure_converter/lidar_points_extended_structure"/>
+ <node pkg="iri_ground_segmentation" type="iri_ground_segmentation" name="iri_ground_segmentation" output="screen" respawn="false">
+ </node>
</launch>
diff --git a/package.xml b/package.xml
index 3e435087400fa895afd3e5d2168353f28199ef0c..681853aa9ed150a13a45e7056bf1d9ee7a19c147 100644
--- a/package.xml
+++ b/package.xml
@@ -63,7 +63,9 @@
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
-
+ <exec_depend>jsk_recognition</exec_depend>
+ <exec_depend>jsk_common_msgs</exec_depend>
+ <exec_depend>jsk_rviz_plugins</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/params/iri_ground_segmentation.yaml b/params/iri_ground_segmentation.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6f4b292e118fff76d3bdde19233a6e0bad3cd00a
--- /dev/null
+++ b/params/iri_ground_segmentation.yaml
@@ -0,0 +1,58 @@
+iri_ground_segmentation: {
+ rate: 10.00,
+
+ # Robot related geometric parameters
+ sensor_height: 1.73, ## Ground is expected to be at z = -1*sensor_height
+ robot_height: 2.00, ## All obstacle points above this value (w.r.t the z ground predicted by the algorithm) are treated as overhanging obstacles
+
+ # Parameters affecting the exploration of the pointcloud
+ ROI_delta_x_and_y: 3.0, #4.0, ## This value sets the size of the ROIs: ROI_size = (2*ROI_delta_x_and_y)^2
+ ROI_shadow_area: 7.0, #6.5, ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
+
+ ground_reference_search_resolution_deg: 12, #40.0, #6.0, #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
+ ## it will affect the number of nodes in the graph: lower values generate more nodes
+
+ elevation_grid_resolution: 2.1, #1.2, #1.5, #1.0 #0.5, ## This value is used to create the "elevation point cloud" which is a reduction of the original pointcloud, where
+ ## only the lowest z values in each cell are stored (along with a vector of indexes pointing to the remaining points
+ ## in the cell, so that the original pointcloud can be recovered or labeled using the info in the "elevation cloud").
+ ## Big values can speed up the algorithm but generates a lower resolution ground model, on the other hand, small values
+ ## can produce outliers because we would like to have only ground points in the elevation cloud (however these outliers
+ ## are usually succesfully rejected by the mahalanobis threshold and the "prior" information)
+
+ # Kalman filter noise parameters
+ ## initial uncertainties
+ z_initial_std_dev: 0.05, ## Uncertainty in z = -1*sensor_height
+ initial_angular_std_dev_deg: 1.5, ## Used to initialize x_slope and y_slope variances
+
+ ## propagation additive noises
+ propagation_z_additive_noise_per_meter: 0.01, ## Uncertainty added to var_z as a function of the distance with the parent vertex
+ propagation_additive_noise_deg_per_meter: 0.4, ## Uncertainty added to x_slope and y_slope as a function of the distance with the parent vertex
+ ## (it is expressed in degrees per meter, but inside the code it is converted to slope per meter)
+ # measurement noise
+ z_observation_std_dev: 0.3, #0.15, #0.3, #0.45, for elevation_grid_resolution = 0.5 ## Uncertainty in the Lidar observations
+
+ # threshold for outlier rejection and classification
+ mahalanobis_threshold: 3.0, #2.7, #3.0, ## To classify points as obstacles or ground (a small value will cause false obstacle points,
+ ## and a big value will increase the number of false ground points)
+
+ # Neural Network related parameters
+ use_neural_network: true,
+ extract_data_to_external_training_of_the_network: false,
+ dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset_rgb_hsv_olbp_10_frame_inc.csv',
+ neural_net_filename_with_global_path: '/media/sf_virtual_box_shared/neural_networks/veg_terrain_and_obs_13_features_39_neurons.csv', #five_classes_13_features_39_neurons.csv'
+ neural_net_number_of_features: 13,
+ neural_net_number_of_neurons_in_hidden_layer: 39,
+ neural_net_number_of_neurons_in_output_layer: 2,
+
+ # labeling parameters
+ max_pred_std_dev_for_labelling: 0.333, ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
+ score_threshold: 0.0, ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
+ classify_not_labeled_points_as_obstacles: true, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
+ ground_threshold_in_not_analyzed_areas: 0.1, ## when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and
+ ## all the points above it and below this threshold will be classified as ground
+
+ # visualization and debug parameters
+ measure_performance: false, ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm
+ show_dense_reconstruction: false, ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)
+ ## or alternatively the "elevation point cloud" (useful for parameter tunning)
+}
diff --git a/params/iri_ground_segmentation_no_random_forest.yaml b/params/iri_ground_segmentation_no_random_forest.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b0f2b2952c5d3578dc4a8aabf4c4d92f3d7e0f6f
--- /dev/null
+++ b/params/iri_ground_segmentation_no_random_forest.yaml
@@ -0,0 +1,49 @@
+iri_ground_segmentation: {
+ rate: 10.00,
+
+ # Robot related geometric parameters
+ sensor_height: 1.73, ## Ground is expected to be at z = -1*sensor_height
+ robot_height: 2.00, ## All obstacle points above this value (w.r.t the z ground predicted by the algorithm) are treated as overhanging obstacles
+
+ # Parameters affecting the exploration of the pointcloud
+ ROI_delta_x_and_y: 3.0, ## This value sets the size of the ROIs: ROI_size = (2*ROI_delta_x_and_y)^2
+ ROI_shadow_area: 5.5, ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
+
+ ground_reference_search_resolution_deg: 40.0, #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
+ ## it will affect the number of nodes in the graph: lower values generate more nodes
+
+ elevation_grid_resolution: 2.1, #0.333, ## This value is used to create the "elevation point cloud" which is a reduction of the original pointcloud, where
+ ## only the lowest z values in each cell are stored (along with a vector of indexes pointing to the remaining points
+ ## in the cell, so that the original pointcloud can be recovered or labeled using the info in the "elevation cloud").
+ ## Big values can speed up the algorithm but generates a lower resolution ground model, on the other hand, small values
+ ## can produce outliers because we would like to have only ground points in the elevation cloud (however these outliers
+ ## are usually succesfully rejected by the mahalanobis threshold and the "prior" information)
+
+ # Kalman filter noise parameters
+ ## initial uncertainties
+ z_initial_std_dev: 0.05, ## Uncertainty in z = -1*sensor_height
+ initial_angular_std_dev_deg: 1.5, ## Used to initialize x_slope and y_slope variances
+
+ ## propagation additive noises
+ propagation_z_additive_noise_per_meter: 0.01, ## Uncertainty added to var_z as a function of the distance with the parent vertex
+ propagation_additive_noise_deg_per_meter: 0.4, ## Uncertainty added to x_slope and y_slope as a function of the distance with the parent vertex
+ ## (it is expressed in degrees per meter, but inside the code it is converted to slope per meter)
+ # measurement noise
+ z_observation_std_dev: 0.15, ## Uncertainty in the Lidar observations
+
+ # threshold for outlier rejection and classification
+ mahalanobis_threshold: 2.7, ## To classify points as obstacles or ground (a small value will cause false obstacle points,
+ ## and a big value will increase the number of false ground points)
+
+ # labeling parameters
+ max_pred_std_dev_for_labelling: 0.5, ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
+ score_threshold: 0.0, ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
+ classify_not_labeled_points_as_obstacles: true, ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
+ ground_threshold_in_not_analyzed_areas: 0.1, ## when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and
+ ## all the points above it and below this threshold will be classified as ground
+
+ # visualization and debug parameters
+ measure_performance: false, ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm
+ show_dense_reconstruction: false, ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)
+ ## or alternatively the "elevation point cloud" (useful for parameter tunning)
+}
diff --git a/src/ground_segmentation_alg.cpp b/src/ground_segmentation_alg.cpp
index 410b623e0ab4f2da97028f9c7525479a250df499..70c807a7abb918fbfd6e1042729e4eee0ce6ea66 100644
--- a/src/ground_segmentation_alg.cpp
+++ b/src/ground_segmentation_alg.cpp
@@ -1,4 +1,6 @@
#include "ground_segmentation_alg.h"
+#include "tgs.hpp"
+#include "aos.hpp"
GroundSegmentationAlgorithm::GroundSegmentationAlgorithm(void)
{
@@ -40,204 +42,306 @@ void GroundSegmentationAlgorithm::segmentGround(
std::vector<std::vector<int>> edges(100000);
std::vector < std::vector < kf_based_terrain_analysis_lib::Edge >> discarded_edges(100000);
- //std::cout << "Calling to processor_.groundSegmentation!" << std::endl;
-
- processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, lidar_pcl_cloud,
- roadmap_vertices_pcl_cloud, edges, discarded_edges,
- ground_dense_reconstruction_pcl_cloud);
+ bool use_travel = false;
+ if (use_travel)
+ {
+ // Using TRAVEL for comparison!
+ pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr ground_pc;
+ pcl::PointCloud<pcl::PointXYZI>::Ptr nonground_pc;
+
+ cloud_in.reset(new pcl::PointCloud<pcl::PointXYZI>());
+ ground_pc.reset(new pcl::PointCloud<pcl::PointXYZI>());
+ nonground_pc.reset(new pcl::PointCloud<pcl::PointXYZI>());
+
+ boost::shared_ptr < travel::TravelGroundSeg < pcl::PointXYZI >> travel_ground_seg;
+
+ travel_ground_seg.reset(new travel::TravelGroundSeg<pcl::PointXYZI>());
+
+ double min_range = 1.0;
+ double max_range = 64.0;
+
+ double resolution = 8.0;
+ int num_iter = 3;
+ int num_lpr = 5;
+ int num_min_pts = 10;
+ double th_seeds = 0.5;
+ double th_dist = 0.125;
+ double th_outlier = 0.3;
+ double th_normal = 0.940;
+ double th_weight = 200;
+ double th_lcc_normal = 0.03;
+ double th_lcc_planar = 0.1;
+ double th_obstacle = 1.0;
+ bool refine_mode = true;
+ bool visualization = true;
+
+ std::cout << "Setting TRAVEL parameters!" << std::endl;
+ travel_ground_seg->setParams(max_range, min_range, resolution, num_iter, num_lpr, num_min_pts, th_seeds, th_dist,
+ th_outlier, th_normal, th_weight, th_lcc_normal, th_lcc_planar, th_obstacle,
+ refine_mode, visualization);
+ std::cout << "Parameters set!" << std::endl;
+
+ std::cout << "Copying input data to XYZI format!" << std::endl;
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud.begin(); i != lidar_pcl_cloud.end(); ++i)
+ {
+ pcl::PointXYZI point;
+ point.x = i->x;
+ point.y = i->y;
+ point.z = i->z;
+ point.intensity = 0;
+ cloud_in->points.push_back(point);
+ }
+ std::cout << "Data copied!" << std::endl;
- //std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
- //std::cout << "Number of edge vectors = " << edges.size() << std::endl;
+ double ground_seg_time = 0.0;
+ std::cout << "Starting TRAVEL ground segmentation!" << std::endl;
+ travel_ground_seg->estimateGround(*cloud_in, *ground_pc, *nonground_pc, ground_seg_time);
+ std::cout << "\033[1;35m Traversable-Ground Seg: " << cloud_in->size() << " -> Ground: " << ground_pc->size()
+ << ", NonGround: " << nonground_pc->size() << "\033[0m" << std::endl;
+ std::cout << "Traversable-Ground Seg time: " << ground_seg_time << std::endl;
- // Now we create the markers for visualization and change the format of edges to implement the interface
- // between the library and the node
+ pcl::KdTreeFLANN < pcl::PointXYZI > kdtree_ground;
+ kdtree_ground.setInputCloud(ground_pc);
- int vertex_id = 0;
- int discarded_vertex_id = 0;
- int edge_id = 0;
- int discarded_edge_id = 0;
+ int K = 1;
+ std::vector<int> ground_pointIdxKNNSearch(K);
+ std::vector<float> ground_pointKNNSquaredDistance(K);
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = roadmap_vertices_pcl_cloud.begin();
- i != roadmap_vertices_pcl_cloud.end(); ++i)
- {
- pcl::PointXYZRGBNormal ground_ref = *i;
+ int index = 0;
+ for (pcl::PointCloud<pcl::PointXYZI>::iterator i = cloud_in->begin(); i != cloud_in->end(); ++i)
+ {
+ kdtree_ground.nearestKSearch(*i, K, ground_pointIdxKNNSearch, ground_pointKNNSquaredDistance);
- // DEBUG!
- //std::cout << "Vertex Pitch = " << ground_ref.data_c[GRND_REF_DATA_C_2_PITCH] << " ; Vertex Roll = "
- // << ground_ref.data_c[GRND_REF_DATA_C_1_ROLL] << std::endl;
+ bool ground = false;
+ if (sqrt(ground_pointKNNSquaredDistance[0]) < 0.001)
+ ground = true;
- assert(
- vertex_id == ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
- && "Something unexpected in GroundSegmentationAlgorithm::segmentGround happened! vertex_id not equal to ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]!");
+ if (ground)
+ {
+ lidar_pcl_cloud.points[index].r = 0;
+ lidar_pcl_cloud.points[index].g = 0;
+ lidar_pcl_cloud.points[index].b = 255;
- //std::cout << "Converting vertex with id = " << vertex_id << " to marker!" << std::endl;
- //std::cout << "Number of edges for vertex " << vertex_id << " = " << edges[ground_ref.data_n[3]].size() << std::endl;
+ lidar_pcl_cloud.points[index].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + 0.995;
+ }
+ else
+ {
+ lidar_pcl_cloud.points[index].r = 0;
+ lidar_pcl_cloud.points[index].g = 255;
+ lidar_pcl_cloud.points[index].b = 0;
- visualization_msgs::Marker vertex;
- vertex.action = visualization_msgs::Marker::ADD;
- vertex.header = common_header;
+ lidar_pcl_cloud.points[index].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.0;
+ }
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
- {
- vertex.ns = "vertices_connected_to_root";
- vertex.id = vertex_id++;
- }
- else
- {
- vertex.ns = "vertices_NOT_connected_to_root";
- vertex.id = discarded_vertex_id++;
+ ++index;
}
+ }
+ else
+ {
+ //std::cout << "Calling to processor_.groundSegmentation!" << std::endl;
- vertex.type = visualization_msgs::Marker::SPHERE;
+ processor_.groundSegmentation(lidar_configuration_, filtering_configuration_, lidar_pcl_cloud,
+ roadmap_vertices_pcl_cloud, edges, discarded_edges,
+ ground_dense_reconstruction_pcl_cloud);
- vertex.pose.position.x = ground_ref.x;
- vertex.pose.position.y = ground_ref.y;
- vertex.pose.position.z = ground_ref.z;
+ //std::cout << "Number of vertices = " << roadmap_vertices_pcl_cloud.points.size();
+ //std::cout << "Number of edge vectors = " << edges.size() << std::endl;
- vertex.pose.orientation.x = 0.0;
- vertex.pose.orientation.y = 0.0;
- vertex.pose.orientation.z = 0.0;
- vertex.pose.orientation.w = 1.0;
+ // Now we create the markers for visualization and change the format of edges to implement the interface
+ // between the library and the node
- vertex.scale.x = 0.15;
- vertex.scale.y = 0.15;
- vertex.scale.z = 0.15;
+ int vertex_id = 0;
+ int discarded_vertex_id = 0;
+ int edge_id = 0;
+ int discarded_edge_id = 0;
- vertex.color.a = 1.0; // Don't forget to set the alpha!
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
- {
- vertex.color.r = 1.0;
- vertex.color.g = 0.66;
- vertex.color.b = 0.0;
- }
- else
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = roadmap_vertices_pcl_cloud.begin();
+ i != roadmap_vertices_pcl_cloud.end(); ++i)
{
- vertex.color.r = 1.0;
- vertex.color.g = 0.0;
- vertex.color.b = 0.0;
- }
- vertex.lifetime = ros::Duration(0.1);
+ pcl::PointXYZRGBNormal ground_ref = *i;
- roadmap_markers.markers.push_back(vertex);
+ // DEBUG!
+ //std::cout << "Vertex Pitch = " << ground_ref.data_c[GRND_REF_DATA_C_2_PITCH] << " ; Vertex Roll = "
+ // << ground_ref.data_c[GRND_REF_DATA_C_1_ROLL] << std::endl;
- iri_ground_segmentation::edges edges_temp;
- for (std::vector<int>::iterator e = edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin();
- e != edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end(); e++)
- {
- edges_temp.edges.push_back(*e);
+ assert(
+ vertex_id == ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]
+ && "Something unexpected in GroundSegmentationAlgorithm::segmentGround happened! vertex_id not equal to ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]!");
- visualization_msgs::Marker edge;
- edge.action = visualization_msgs::Marker::ADD;
- edge.header = common_header;
+ //std::cout << "Converting vertex with id = " << vertex_id << " to marker!" << std::endl;
+ //std::cout << "Number of edges for vertex " << vertex_id << " = " << edges[ground_ref.data_n[3]].size() << std::endl;
- if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ // TODO: comment out the marker array creation to see how much time it consumes
+ visualization_msgs::Marker vertex;
+ vertex.action = visualization_msgs::Marker::ADD;
+ vertex.header = common_header;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] < VERTEX_NOT_CONNECTED_TO_ROOT)
{
- edge.ns = "edges";
- edge.id = edge_id++;
+ vertex.ns = "vertices_connected_to_root";
+ vertex.id = vertex_id++;
}
else
{
- edge.ns = "discarded_edges";
- edge.id = discarded_edge_id++;
+ vertex.ns = "vertices_NOT_connected_to_root";
+ vertex.id = discarded_vertex_id++;
}
- edge.type = visualization_msgs::Marker::LINE_LIST;
-
- geometry_msgs::Point p;
- p.x = ground_ref.x;
- p.y = ground_ref.y;
- p.z = ground_ref.z;
- edge.points.push_back(p);
+ vertex.type = visualization_msgs::Marker::SPHERE;
- pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
- p.x = child_ground_ref.x;
- p.y = child_ground_ref.y;
- p.z = child_ground_ref.z;
- edge.points.push_back(p);
+ vertex.pose.position.x = ground_ref.x;
+ vertex.pose.position.y = ground_ref.y;
+ vertex.pose.position.z = ground_ref.z;
- edge.scale.x = 0.05;
+ vertex.pose.orientation.x = 0.0;
+ vertex.pose.orientation.y = 0.0;
+ vertex.pose.orientation.z = 0.0;
+ vertex.pose.orientation.w = 1.0;
- edge.pose.orientation.x = 0.0;
- edge.pose.orientation.y = 0.0;
- edge.pose.orientation.z = 0.0;
- edge.pose.orientation.w = 1.0;
+ vertex.scale.x = 0.25;
+ vertex.scale.y = 0.25;
+ vertex.scale.z = 0.25;
- edge.color.a = 0.7; // Don't forget to set the alpha!
+ vertex.color.a = 1.0; // Don't forget to set the alpha!
if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
{
- edge.color.r = 0.8;
- edge.color.g = 0.53;
- edge.color.b = 0.0;
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.66;
+ vertex.color.b = 0.0;
}
else
{
- edge.color.r = R_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.g = G_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
- edge.color.b = B_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ vertex.color.r = 1.0;
+ vertex.color.g = 0.0;
+ vertex.color.b = 0.0;
}
- edge.lifetime = ros::Duration(0.1);
+ vertex.lifetime = ros::Duration(0); //0.1
- roadmap_markers.markers.push_back(edge);
- }
+ roadmap_markers.markers.push_back(vertex);
- edges_array.edges_array.push_back(edges_temp);
+ iri_ground_segmentation::edges edges_temp;
+ for (std::vector<int>::iterator e = edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].begin();
+ e != edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].end(); e++)
+ {
+ edges_temp.edges.push_back(*e);
+
+ pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
+
+ visualization_msgs::Marker edge;
+ edge.action = visualization_msgs::Marker::ADD;
+ edge.header = common_header;
+
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT &&
+ child_ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ {
+ edge.ns = "edges";
+ edge.id = edge_id++;
+ }
+ else
+ {
+ edge.ns = "discarded_edges";
+ edge.id = discarded_edge_id++;
+ }
+
+ edge.type = visualization_msgs::Marker::LINE_LIST;
+
+ geometry_msgs::Point p;
+ p.x = ground_ref.x;
+ p.y = ground_ref.y;
+ p.z = ground_ref.z;
+ edge.points.push_back(p);
+
+ p.x = child_ground_ref.x;
+ p.y = child_ground_ref.y;
+ p.z = child_ground_ref.z;
+ edge.points.push_back(p);
+
+ edge.scale.x = 0.1;
+
+ edge.pose.orientation.x = 0.0;
+ edge.pose.orientation.y = 0.0;
+ edge.pose.orientation.z = 0.0;
+ edge.pose.orientation.w = 1.0;
+
+ edge.color.a = 1.0; // Don't forget to set the alpha!
+ if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT &&
+ child_ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+ {
+ edge.color.r = 0.8;
+ edge.color.g = 0.53;
+ edge.color.b = 0.0;
+ }
+ else
+ {
+ edge.color.r = R_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.g = G_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ edge.color.b = B_VERTEX_NOT_CONNECTED_TO_ROOT / 255.0;
+ }
+ edge.lifetime = ros::Duration(0); // 0.1
+
+ roadmap_markers.markers.push_back(edge);
+ }
- // We create markers for the discarded edges
- for (int e = 0; e < discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size(); e++)
- {
- kf_based_terrain_analysis_lib::Edge discarded_edge_index_and_class =
- discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]][e];
-
- visualization_msgs::Marker discarded_edge;
- discarded_edge.action = visualization_msgs::Marker::ADD;
- discarded_edge.header = common_header;
- discarded_edge.ns = "discarded_edges";
- discarded_edge.id = discarded_edge_id++;
- discarded_edge.type = visualization_msgs::Marker::LINE_LIST;
-
- geometry_msgs::Point p;
- p.x = ground_ref.x;
- p.y = ground_ref.y;
- p.z = ground_ref.z;
- discarded_edge.points.push_back(p);
-
- pcl::PointXYZRGBNormal child_ground_ref =
- roadmap_vertices_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
- p.x = child_ground_ref.x;
- p.y = child_ground_ref.y;
- p.z = child_ground_ref.z;
- discarded_edge.points.push_back(p);
-
- discarded_edge.scale.x = 0.05;
-
- discarded_edge.pose.orientation.x = 0.0;
- discarded_edge.pose.orientation.y = 0.0;
- discarded_edge.pose.orientation.z = 0.0;
- discarded_edge.pose.orientation.w = 1.0;
-
- discarded_edge.color.a = 0.7;
- switch (discarded_edge_index_and_class.edge_class)
+ edges_array.edges_array.push_back(edges_temp);
+
+ // We create markers for the discarded edges
+ for (int e = 0; e < discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size(); e++)
{
- case CLASS_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE:
- //std::cout << "Coloring edge too much Mahalanobis distance! class = "
- // << discarded_edge_index_and_class.edge_class << std::endl;
- discarded_edge.color.r = (float)R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
- break;
- default:
- //std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class
- // << std::endl;
- discarded_edge.color.r = 1.0;
- discarded_edge.color.g = 1.0;
- discarded_edge.color.b = 1.0;
- break;
+ kf_based_terrain_analysis_lib::Edge discarded_edge_index_and_class =
+ discarded_edges[ground_ref.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]][e];
+
+ visualization_msgs::Marker discarded_edge;
+ discarded_edge.action = visualization_msgs::Marker::ADD;
+ discarded_edge.header = common_header;
+ discarded_edge.ns = "discarded_edges";
+ discarded_edge.id = discarded_edge_id++;
+ discarded_edge.type = visualization_msgs::Marker::LINE_LIST;
+
+ geometry_msgs::Point p;
+ p.x = ground_ref.x;
+ p.y = ground_ref.y;
+ p.z = ground_ref.z;
+ discarded_edge.points.push_back(p);
+
+ pcl::PointXYZRGBNormal child_ground_ref =
+ roadmap_vertices_pcl_cloud.points[discarded_edge_index_and_class.vertex_index];
+ p.x = child_ground_ref.x;
+ p.y = child_ground_ref.y;
+ p.z = child_ground_ref.z;
+ discarded_edge.points.push_back(p);
+
+ discarded_edge.scale.x = 0.1;
+
+ discarded_edge.pose.orientation.x = 0.0;
+ discarded_edge.pose.orientation.y = 0.0;
+ discarded_edge.pose.orientation.z = 0.0;
+ discarded_edge.pose.orientation.w = 1.0;
+
+ discarded_edge.color.a = 1.0;
+ switch (discarded_edge_index_and_class.edge_class)
+ {
+ case CLASS_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE:
+ //std::cout << "Coloring edge too much Mahalanobis distance! class = "
+ // << discarded_edge_index_and_class.edge_class << std::endl;
+ discarded_edge.color.r = (float)R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.g = (float)G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ discarded_edge.color.b = (float)B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE / 255.0;
+ break;
+ default:
+ //std::cout << "WARNING! Discarded edge class not known!! class = " << discarded_edge_index_and_class.edge_class
+ // << std::endl;
+ discarded_edge.color.r = 1.0;
+ discarded_edge.color.g = 1.0;
+ discarded_edge.color.b = 1.0;
+ break;
+ }
+ discarded_edge.lifetime = ros::Duration(0); // 0.1
+ roadmap_markers.markers.push_back(discarded_edge);
}
- discarded_edge.lifetime = ros::Duration(0.1);
- roadmap_markers.markers.push_back(discarded_edge);
}
- }
- //std::cout << "Total number of edges = " << edge_id << std::endl;
- //getchar();
+ //std::cout << "Total number of edges = " << edge_id << std::endl;
+ //getchar();
+ }
}
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 13b9c38a0fef93e9488ddf52c1bca916b8d35a99..0040cfd9958f53d894a898ec17877c02e8046174 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -34,6 +34,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.ROI_delta_x_and_y = config_.ROI_delta_x_and_y;
+ if (!this->private_node_handle_.getParam("ROI_shadow_area", this->config_.ROI_shadow_area))
+ {
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ROI_shadow_area' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.ROI_shadow_area = config_.ROI_shadow_area;
+
if (!this->private_node_handle_.getParam("ground_reference_search_resolution_deg",
this->config_.ground_reference_search_resolution_deg))
{
@@ -106,17 +113,75 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.mahalanobis_threshold = config_.mahalanobis_threshold;
- ////////////////// labeling parameters
- if (!this->private_node_handle_.getParam("number_of_references_used_for_labelling",
- this->config_.number_of_references_used_for_labelling))
+ /////////////////// Neural Network parameters
+ if (!this->private_node_handle_.getParam("use_neural_network", this->config_.use_neural_network))
+ {
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'use_neural_network' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.use_neural_network = config_.use_neural_network;
+
+ if (!this->private_node_handle_.getParam("extract_data_to_external_training_of_the_network",
+ this->config_.extract_data_to_external_training_of_the_network))
+ {
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'extract_data_to_external_training_of_the_network' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network =
+ config_.extract_data_to_external_training_of_the_network;
+
+ if (!this->private_node_handle_.getParam("dataset_filename_with_global_path",
+ this->config_.dataset_filename_with_global_path))
+ {
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'dataset_filename_with_global_path' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.dataset_filename_with_global_path =
+ config_.dataset_filename_with_global_path;
+
+ if (!this->private_node_handle_.getParam("neural_net_filename_with_global_path",
+ this->config_.neural_net_filename_with_global_path))
{
ROS_WARN(
- "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'number_of_references_used_for_labelling' not found");
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_filename_with_global_path' not found");
}
else
- this->alg_.filtering_configuration_.number_of_references_used_for_labelling =
- config_.number_of_references_used_for_labelling;
+ this->alg_.filtering_configuration_.neural_net_filename_with_global_path =
+ config_.neural_net_filename_with_global_path;
+ if (!this->private_node_handle_.getParam("neural_net_number_of_features",
+ this->config_.neural_net_number_of_features))
+ {
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_features' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.neural_net_number_of_features =
+ config_.neural_net_number_of_features;
+
+ if (!this->private_node_handle_.getParam("neural_net_number_of_neurons_in_hidden_layer",
+ this->config_.neural_net_number_of_neurons_in_hidden_layer))
+ {
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_neurons_in_hidden_layer' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.neural_net_number_of_neurons_in_hidden_layer =
+ config_.neural_net_number_of_neurons_in_hidden_layer;
+
+ if (!this->private_node_handle_.getParam("neural_net_number_of_neurons_in_output_layer",
+ this->config_.neural_net_number_of_neurons_in_output_layer))
+ {
+ ROS_WARN(
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'neural_net_number_of_neurons_in_output_layer' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.neural_net_number_of_neurons_in_output_layer =
+ config_.neural_net_number_of_neurons_in_output_layer;
+
+ ////////////////// labeling parameters
if (!this->private_node_handle_.getParam("max_pred_std_dev_for_labelling",
this->config_.max_pred_std_dev_for_labelling))
{
@@ -125,6 +190,13 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config_.max_pred_std_dev_for_labelling;
+ if (!this->private_node_handle_.getParam("score_threshold", this->config_.score_threshold))
+ {
+ ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'score_threshold' not found");
+ }
+ else
+ this->alg_.filtering_configuration_.score_threshold = config_.score_threshold;
+
if (!this->private_node_handle_.getParam("classify_not_labeled_points_as_obstacles",
this->config_.classify_not_labeled_points_as_obstacles))
{
@@ -135,15 +207,15 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
config_.classify_not_labeled_points_as_obstacles;
- if (!this->private_node_handle_.getParam("discard_not_connected_references_for_labelling",
- this->config_.discard_not_connected_references_for_labelling))
+ if (!this->private_node_handle_.getParam("ground_threshold_in_not_analyzed_areas",
+ this->config_.ground_threshold_in_not_analyzed_areas))
{
ROS_WARN(
- "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'discard_not_connected_references_for_labelling' not found");
+ "GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'ground_threshold_in_not_analyzed_areas' not found");
}
else
- this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling =
- config_.discard_not_connected_references_for_labelling;
+ this->alg_.filtering_configuration_.ground_threshold_in_not_analyzed_areas =
+ config_.ground_threshold_in_not_analyzed_areas;
////////////////// visualization and debug parameters
if (!this->private_node_handle_.getParam("measure_performance", this->config_.measure_performance))
@@ -161,8 +233,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
this->alg_.filtering_configuration_.show_dense_reconstruction = config_.show_dense_reconstruction;
// [init subscribers]
- this->lidar_subscriber_ = this->public_node_handle_.subscribe("/lidar_points_extended_structure", 1,
- &GroundSegmentationAlgNode::cb_lidar, this);
+ this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points_extended_structure", 1,
+ &GroundSegmentationAlgNode::cb_lidar, this);
// [init publishers]
@@ -194,6 +266,8 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
flag_new_lidar_data_ = false;
pthread_mutex_init(&this->mutex_, NULL);
// [init services]
+
+ std::cout << "Ground segmentation node instantiated!!" << std::endl;
}
GroundSegmentationAlgNode::~GroundSegmentationAlgNode(void)
@@ -269,13 +343,14 @@ void GroundSegmentationAlgNode::unmutexAll(void)
bool GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived(void)
{
bool ready_to_process = false;
-
+ this->mutex_enter();
if (flag_new_lidar_data_)
{
ready_to_process = true;
//std::cout << "GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived..." << std::endl;
//std::cout << "Ready_to_process = " << ready_to_process << std::endl;
}
+ this->mutex_exit();
return (ready_to_process);
}
@@ -288,6 +363,24 @@ void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
+// std::cout << "x = " << lidar_pcl_cloud_.points[100].x << std::endl;
+// std::cout << "y = " << lidar_pcl_cloud_.points[100].y << std::endl;
+// std::cout << "z = " << lidar_pcl_cloud_.points[100].z << std::endl;
+//
+// std::cout << "r = " << (int)lidar_pcl_cloud_.points[100].r << std::endl;
+// std::cout << "g = " << (int)lidar_pcl_cloud_.points[100].g << std::endl;
+// std::cout << "b = " << (int)lidar_pcl_cloud_.points[100].b << std::endl;
+//
+// std::cout << "DATA_N_0_INTENSITY = " << lidar_pcl_cloud_.points[100].data_n[DATA_N_0_INTENSITY] << std::endl;
+// std::cout << "DATA_N_1_INTENSITY_VARIANCE = " << lidar_pcl_cloud_.points[100].data_n[DATA_N_1_INTENSITY_VARIANCE] << std::endl;
+// std::cout << "DATA_N_2_Z_MEAN = " << lidar_pcl_cloud_.points[100].data_n[DATA_N_2_Z_MEAN] << std::endl;
+// std::cout << "DATA_N_3_Z_VARIANCE = " << lidar_pcl_cloud_.points[100].data_n[DATA_N_3_Z_VARIANCE] << std::endl;
+//
+// std::cout << "DATA_C_0_RGB_CAST_INTO_FLOAT = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] << std::endl;
+// std::cout << "DATA_C_1_ID_CLASS = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_1_ID_CLASS] << std::endl;
+// std::cout << "DATA_C_2_MEAN_INTENSITY = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_2_MEAN_INTENSITY] << std::endl;
+// std::cout << "DATA_C_3_ORIGINAL_INDEX = " << lidar_pcl_cloud_.points[100].data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
+
flag_new_lidar_data_ = false;
}
@@ -310,9 +403,15 @@ void GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass(void)
case CLASS_OBSTACLE:
obstacle_points_pcl_cloud_.push_back(regular_point);
break;
+ case CLASS_OVERHANGING_OBSTACLE:
+ // If we want to create a pointcloud with the overhanging obstacles, we should populate it here
+ break;
+ case CLASS_UNKNOWN:
+ // If we ever want to create a pointcloud with unknown class points, we should populate it here
+ break;
default:
- // std::cout << "GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass -> unexpected class value of: "
- // << classified_point.data_c[DATA_C_1_ID_CLASS] << std::endl;
+ //std::cout << "GroundSegmentationAlgNode::splitSegmentedCloudInOnePerClass -> unexpected class value of: "
+ // << classified_point.data_c[DATA_C_1_ID_CLASS] << std::endl;
break;
}
}
@@ -351,6 +450,13 @@ void GroundSegmentationAlgNode::publishAll(void)
// Visualization topics
this->ground_dense_reconstruction_publisher_.publish(this->ground_dense_reconstruction_ros_cloud_);
+
+
+ visualization_msgs::Marker markerD;
+ markerD.header = common_header_;
+ markerD.action = visualization_msgs::Marker::DELETEALL;
+ this->roadmap_markers_publisher_.publish(markerD);
+
this->roadmap_markers_publisher_.publish(this->roadmap_MarkerArray_msg_);
}
@@ -395,8 +501,12 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
this->mutex_exit(); // Then we can free the callback
+ this->alg_.lock();
+ //std::cout << "Starting ground segmentation!" << std::endl;
this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_,
ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
+ //std::cout << "Ground segmentation finished!" << std::endl;
+ this->alg_.unlock();
this->convertOutputsToSuitableFormats();
this->publishAll();
@@ -408,13 +518,13 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
void GroundSegmentationAlgNode::cb_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
+ this->mutex_enter();
lidar_ros_cloud_ = *msg;
//std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
- this->mutex_enter();
- flag_new_lidar_data_ = true; // This flag is shared with the main node thread, as the lidar_ros_cloud, but the latter is protected
- // by the flag, so it is not required to use the mutex
+ flag_new_lidar_data_ = true;
+
this->mutex_exit();
}
@@ -448,6 +558,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
// Parameters affecting the exploration of the pointcloud
this->alg_.filtering_configuration_.ROI_delta_x_and_y = config.ROI_delta_x_and_y;
+ this->alg_.filtering_configuration_.ROI_shadow_area = config.ROI_shadow_area;
this->alg_.filtering_configuration_.ground_reference_search_resolution = config.ground_reference_search_resolution_deg
* M_PI / 180.0;
this->alg_.filtering_configuration_.elevation_grid_resolution = config.elevation_grid_resolution;
@@ -467,14 +578,16 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
// threshold for outlier rejection and classification
this->alg_.filtering_configuration_.mahalanobis_threshold = config.mahalanobis_threshold;
+ // Neural Network parameters
+ this->alg_.filtering_configuration_.use_neural_network = config.use_neural_network;
+ this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network =
+ config.extract_data_to_external_training_of_the_network;
+
// labeling parameters
- this->alg_.filtering_configuration_.number_of_references_used_for_labelling =
- config.number_of_references_used_for_labelling;
+ this->alg_.filtering_configuration_.score_threshold = config_.score_threshold;
this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling = config.max_pred_std_dev_for_labelling;
this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
config_.classify_not_labeled_points_as_obstacles;
- this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling =
- config_.discard_not_connected_references_for_labelling;
// visualization and debug parameters
this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
@@ -490,6 +603,10 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "ROI_delta_x_and_y = " << this->alg_.filtering_configuration_.ROI_delta_x_and_y
<< std::endl;
+
+ std::cout << "ROI_shadow_area = " << this->alg_.filtering_configuration_.ROI_shadow_area
+ << std::endl;
+
std::cout << "ground_reference_search_resolution (deg) = "
<< this->alg_.filtering_configuration_.ground_reference_search_resolution * 180.0 / M_PI << std::endl;
std::cout << "elevation_grid_resolution = "
@@ -511,14 +628,19 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "mahalanobis_threshold = "
<< this->alg_.filtering_configuration_.mahalanobis_threshold << std::endl;
- std::cout << "number_of_references_used_for_labelling = "
- << this->alg_.filtering_configuration_.number_of_references_used_for_labelling << std::endl;
+ std::cout << "score_threshold = "
+ << this->alg_.filtering_configuration_.score_threshold << std::endl;
+
+ std::cout << "use_neural_network = "
+ << this->alg_.filtering_configuration_.use_neural_network << std::endl;
+
+ std::cout << "extract_data_to_external_training_of_the_network = "
+ << this->alg_.filtering_configuration_.extract_data_to_external_training_of_the_network << std::endl;
+
std::cout << "max_pred_std_dev_for_labelling = "
<< this->alg_.filtering_configuration_.max_pred_std_dev_for_labelling << std::endl;
std::cout << "classify_not_labeled_points_as_obstacles = "
<< this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles << std::endl;
- std::cout << "discard_not_connected_references_for_labelling = "
- << this->alg_.filtering_configuration_.discard_not_connected_references_for_labelling << std::endl;
std::cout << "measure_performance = " << this->alg_.filtering_configuration_.measure_performance << std::endl;
std::cout << "show_dense_reconstruction = " << this->alg_.filtering_configuration_.show_dense_reconstruction