add detection_lidar_cnn_segmentation_trt.

src/detection/detection_lidar_cnn_segmentation_trt/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/detection/detection_lidar_cnn_segmentation_trt/detection_lidar_cnn_segmentation_trt.pro

@@ -0,0 +1,79 @@
+QT -= gui
+
+CONFIG += c++14  # console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        lib/src/TrtNet.cpp \
+        main.cpp \
+        src/cluster2d.cpp \
+        src/debugger.cpp \
+        src/detector.cpp \
+        src/feature_generator.cpp \
+        src/feature_map.cpp \
+        src/log_table.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    include/lidar_apollo_instance_segmentation/cluster2d.hpp \
+    include/lidar_apollo_instance_segmentation/debugger.hpp \
+    include/lidar_apollo_instance_segmentation/detector.hpp \
+    include/lidar_apollo_instance_segmentation/disjoint_set.hpp \
+    include/lidar_apollo_instance_segmentation/feature_generator.hpp \
+    include/lidar_apollo_instance_segmentation/feature_map.hpp \
+    include/lidar_apollo_instance_segmentation/log_table.hpp \
+    include/lidar_apollo_instance_segmentation/util.hpp \
+    lib/include/TrtNet.hpp \
+    lib/include/Utils.hpp
+
+
+INCLUDEPATH += $$PWD/include
+
+INCLUDEPATH += $$PWD/lib/include
+
+INCLUDEPATH += /usr/include/pcl-1.10
+
+INCLUDEPATH += /usr/include/eigen3
+
+INCLUDEPATH += /usr/local/cuda-11.4/targets/aarch64-linux/include
+
+LIBS += -L/usr/local/cuda-11.4/targets/aarch64-linux/lib  # -lcublas
+
+LIBS += -lnvinfer -lcudnn  -lcudart -lnvparsers -lnvcaffe_parser -lnvinfer_plugin
+
+
+LIBS += -lboost_system -lboost_thread
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/cluster2d.hpp

@@ -0,0 +1,199 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_
+#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_
+
+#include "disjoint_set.hpp"
+#include "util.hpp"
+
+//#include <std_msgs/msg/header.hpp>
+//#include <tier4_perception_msgs/msg/detected_object_with_feature.hpp>
+//#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <memory>
+#include <vector>
+
+
+enum ObjectType
+{
+  UNKNOWN = 0,
+  UNKNOWN_MOVABLE = 1,
+  UNKNOWN_UNMOVABLE = 2,
+  PEDESTRIAN = 3,
+  BICYCLE = 4,
+  VEHICLE = 5,
+  MAX_OBJECT_TYPE = 6,
+};
+
+
+enum MetaType {
+  META_UNKNOWN,
+  META_SMALL_MOT,
+  META_BIG_MOT,
+  META_NON_MOT,
+  META_PEDESTRIAN,
+  MAX_META_TYPE
+};
+
+struct Obstacle
+{
+  std::vector<int> grids;
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr;
+  float score;
+  float height;
+  float heading;
+  MetaType meta_type;
+  std::vector<float> meta_type_probs;
+
+  Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN)
+  {
+    cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
+    meta_type_probs.assign(MAX_META_TYPE, 0.0);
+  }
+
+  std::string GetTypeString() const
+  {
+    switch (meta_type)
+    {
+      case META_UNKNOWN:
+        return "unknown";
+      case META_SMALL_MOT:
+        return "car";
+      case META_BIG_MOT:
+        return "car";
+      case META_NON_MOT:
+        return "bike";
+      case META_PEDESTRIAN:
+        return "pedestrian";
+      default:
+        return "unknown";
+    }
+  }
+};
+
+class Cluster2D
+{
+public:
+  Cluster2D(const int rows, const int cols, const float range);
+
+  ~Cluster2D() {}
+
+  void cluster(
+    const std::shared_ptr<float> & inferred_data,
+    const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr, const pcl::PointIndices & valid_indices,
+    float objectness_thresh, bool use_all_grids_for_clustering);
+
+  void filter(const std::shared_ptr<float> & inferred_data);
+  void classify(const std::shared_ptr<float> & inferred_data);
+
+
+  void getObjectVector(const float confidence_thresh,
+                       const float height_thresh,
+                       const int min_pts_num,std::vector<Obstacle> & objvec);
+
+//  void getObjects(
+//    const float confidence_thresh, const float height_thresh, const int min_pts_num,
+//    tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects,
+//    const std_msgs::msg::Header & in_header);
+
+//  tier4_perception_msgs::msg::DetectedObjectWithFeature obstacleToObject(
+//    const Obstacle & in_obstacle, const std_msgs::msg::Header & in_header);
+
+private:
+  int rows_;
+  int cols_;
+  int siz_;
+  float range_;
+  float scale_;
+  float inv_res_x_;
+  float inv_res_y_;
+  std::vector<int> point2grid_;
+  std::vector<Obstacle> obstacles_;
+  std::vector<int> id_img_;
+
+  pcl::PointCloud<pcl::PointXYZI>::Ptr pc_ptr_;
+  const std::vector<int> * valid_indices_in_pc_ = nullptr;
+
+  struct Node
+  {
+    Node * center_node;
+    Node * parent;
+    char node_rank;
+    char traversed;
+    bool is_center;
+    bool is_object;
+    int point_num;
+    int obstacle_id;
+
+    Node()
+    {
+      center_node = nullptr;
+      parent = nullptr;
+      node_rank = 0;
+      traversed = 0;
+      is_center = false;
+      is_object = false;
+      point_num = 0;
+      obstacle_id = -1;
+    }
+  };
+
+  inline bool IsValidRowCol(int row, int col) const { return IsValidRow(row) && IsValidCol(col); }
+
+  inline bool IsValidRow(int row) const { return row >= 0 && row < rows_; }
+
+  inline bool IsValidCol(int col) const { return col >= 0 && col < cols_; }
+
+  inline int RowCol2Grid(int row, int col) const { return row * cols_ + col; }
+
+  void traverse(Node * x);
+};
+
+#endif  // LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/debugger.hpp

@@ -0,0 +1,32 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+//#include <rclcpp/rclcpp.hpp>
+
+//#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
+class Debugger
+{
+public:
+  explicit Debugger();
+//  explicit Debugger(rclcpp::Node * node);
+  ~Debugger() {}
+//  void publishColoredPointCloud(
+//    const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input);
+
+private:
+//  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr instance_pointcloud_pub_;
+};

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/detector.hpp

@@ -0,0 +1,64 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "cluster2d.hpp"
+#include "feature_generator.hpp"
+//#include "lidar_apollo_instance_segmentation/node.hpp"
+
+#include <TrtNet.hpp>
+
+#include <pcl/common/transforms.h>
+//#ifdef ROS_DISTRO_GALACTIC
+//#include <tf2_eigen/tf2_eigen.h>
+//#else
+//#include <tf2_eigen/tf2_eigen.hpp>
+//#endif
+//#include <tf2_ros/buffer_interface.h>
+//#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+
+class LidarApolloInstanceSegmentation
+{
+public:
+  explicit LidarApolloInstanceSegmentation();
+  ~LidarApolloInstanceSegmentation() {}
+
+
+   bool detectDynamicObjects(
+      const pcl::PointCloud<pcl::PointXYZI>::Ptr &pcl_pointcloud_raw_ptr,
+      std::vector<Obstacle> & objvec);
+//  bool detectDynamicObjects(
+//    const sensor_msgs::msg::PointCloud2 & input,
+//    tier4_perception_msgs::msg::DetectedObjectsWithFeature & output) override;
+
+private:
+//  bool transformCloud(
+//    const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud,
+//    float z_offset);
+
+//  rclcpp::Node * node_;
+  std::unique_ptr<Tn::trtNet> net_ptr_;
+  std::shared_ptr<Cluster2D> cluster2d_;
+  std::shared_ptr<FeatureGenerator> feature_generator_;
+  float score_threshold_;
+
+//  tf2_ros::Buffer tf_buffer_;
+//  tf2_ros::TransformListener tf_listener_;
+  std::string target_frame_;
+  float z_offset_;
+};

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/disjoint_set.hpp

@@ -0,0 +1,75 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_
+#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_
+
+template <class T>
+void DisjointSetMakeSet(T * x)
+{
+  x->parent = x;
+  x->node_rank = 0;
+}
+
+template <class T>
+T * DisjointSetFindRecursive(T * x)
+{
+  if (x->parent != x) {
+    x->parent = DisjointSetFindRecursive(x->parent);
+  }
+  return x->parent;
+}
+
+template <class T>
+T * DisjointSetFind(T * x)
+{
+  T * y = x->parent;
+  if (y == x || y->parent == y) {
+    return y;
+  }
+  T * root = DisjointSetFindRecursive(y->parent);
+  x->parent = root;
+  y->parent = root;
+  return root;
+}
+
+template <class T>
+void DisjointSetMerge([[maybe_unused]] T * x, [[maybe_unused]] const T * y)
+{
+}
+
+template <class T>
+void DisjointSetUnion(T * x, T * y)
+{
+  x = DisjointSetFind(x);
+  y = DisjointSetFind(y);
+  if (x == y) {
+    return;
+  }
+  if (x->node_rank < y->node_rank) {
+    x->parent = y;
+    DisjointSetMerge(y, x);
+  } else if (y->node_rank < x->node_rank) {
+    y->parent = x;
+    DisjointSetMerge(x, y);
+  } else {
+    y->parent = x;
+    x->node_rank++;
+    DisjointSetMerge(x, y);
+  }
+}
+
+#endif  // LIDAR_APOLLO_INSTANCE_SEGMENTATION__DISJOINT_SET_HPP_

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/feature_generator.hpp

@@ -0,0 +1,42 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "lidar_apollo_instance_segmentation/feature_map.hpp"
+#include "util.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <memory>
+
+class FeatureGenerator
+{
+private:
+  float min_height_;
+  float max_height_;
+  bool use_intensity_feature_;
+  bool use_constant_feature_;
+  std::shared_ptr<FeatureMapInterface> map_ptr_;
+
+public:
+  FeatureGenerator(
+    const int width, const int height, const int range, const bool use_intensity_feature,
+    const bool use_constant_feature);
+  ~FeatureGenerator() {}
+
+  std::shared_ptr<FeatureMapInterface> generate(
+    const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr);
+};

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/feature_map.hpp

@@ -0,0 +1,65 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+#include <memory>
+#include <vector>
+
+struct FeatureMapInterface
+{
+public:
+  int channels;
+  int width;
+  int height;
+  int range;
+  float * max_height_data;      // channel 0
+  float * mean_height_data;     // channel 1
+  float * count_data;           // channel 2
+  float * direction_data;       // channel 3
+  float * top_intensity_data;   // channel 4
+  float * mean_intensity_data;  // channel 5
+  float * distance_data;        // channel 6
+  float * nonempty_data;        // channel 7
+  std::vector<float> map_data;
+  virtual void initializeMap(std::vector<float> & map) = 0;
+  virtual void resetMap(std::vector<float> & map) = 0;
+  FeatureMapInterface(const int _channels, const int _width, const int _height, const int _range);
+};
+
+struct FeatureMap : public FeatureMapInterface
+{
+  FeatureMap(const int width, const int height, const int range);
+  void initializeMap(std::vector<float> & map) override;
+  void resetMap(std::vector<float> & map) override;
+};
+
+struct FeatureMapWithIntensity : public FeatureMapInterface
+{
+  FeatureMapWithIntensity(const int width, const int height, const int range);
+  void initializeMap(std::vector<float> & map) override;
+  void resetMap(std::vector<float> & map) override;
+};
+
+struct FeatureMapWithConstant : public FeatureMapInterface
+{
+  FeatureMapWithConstant(const int width, const int height, const int range);
+  void initializeMap(std::vector<float> & map) override;
+  void resetMap(std::vector<float> & map) override;
+};
+
+struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface
+{
+  FeatureMapWithConstantAndIntensity(const int width, const int height, const int range);
+  void initializeMap(std::vector<float> & map) override;
+  void resetMap(std::vector<float> & map) override;
+};

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/log_table.hpp

@@ -0,0 +1,16 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+
+float calcApproximateLog(float num);

src/detection/detection_lidar_cnn_segmentation_trt/include/lidar_apollo_instance_segmentation/util.hpp

@@ -0,0 +1,59 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_
+#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_
+
+#include <cmath>
+#include <string>
+
+// project point cloud to 2d map. calc in which grid point is.
+// pointcloud to pixel
+inline int F2I(float val, float ori, float scale)
+{
+  return static_cast<int>(std::floor((ori - val) * scale));
+}
+
+inline int Pc2Pixel(float in_pc, float in_range, float out_size)
+{
+  float inv_res = 0.5 * out_size / in_range;
+  return static_cast<int>(std::floor((in_range - in_pc) * inv_res));
+}
+
+// return the distance from my car to center of the grid.
+// Pc means point cloud = real world scale. so transform pixel scale to real
+// world scale
+inline float Pixel2Pc(int in_pixel, float in_size, float out_range)
+{
+  float res = 2.0 * out_range / in_size;
+  return out_range - (static_cast<float>(in_pixel) + 0.5f) * res;
+}
+
+#endif  // LIDAR_APOLLO_INSTANCE_SEGMENTATION__UTIL_HPP_

src/detection/detection_lidar_cnn_segmentation_trt/main.cpp

@@ -0,0 +1,64 @@
+#include <QCoreApplication>
+
+
+#include <pcl/io/pcd_io.h>
+
+#include <chrono>
+
+#include "lidar_apollo_instance_segmentation/detector.hpp"
+LidarApolloInstanceSegmentation * gseg;
+
+void test()
+{
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr xpc(
+        new pcl::PointCloud<pcl::PointXYZI>);
+
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr xpcfile;
+
+    xpcfile = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>>(new pcl::PointCloud<pcl::PointXYZI>);
+
+    if(0 == pcl::io::loadPCDFile("/home/nvidia/share/middle.pcd",*xpc))
+    {
+
+    }
+    else
+    {
+        std::cout<<"load pcd fail"<<std::endl;
+        return;
+    }
+
+    int i;
+    for(i=0;i<1000;i++)
+    {
+
+    int64_t time1 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+
+
+    std::vector<Obstacle>  objvec;
+    gseg->detectDynamicObjects(
+                xpc,
+                objvec);
+
+    int64_t time2 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+    std::cout<<"complete test. use "<<(time2 - time1)/1000<< std::endl;
+
+    }
+    std::cout<<"test complete."<<std::endl;
+}
+
+//LidarApolloInstanceSegmentation
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    gseg = new LidarApolloInstanceSegmentation();
+
+    test();
+
+    return a.exec();
+}

src/detection/detection_lidar_cnn_segmentation_trt/src/cluster2d.cpp

@@ -0,0 +1,428 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "lidar_apollo_instance_segmentation/cluster2d.hpp"
+
+//#include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
+//#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+
+//#include <pcl_conversions/pcl_conversions.h>
+//#ifdef ROS_DISTRO_GALACTIC
+//#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+//#else
+//#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+//#endif
+
+
+#include <memory>
+
+//geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y)
+//{
+//  tf2::Quaternion q;
+//  q.setRPY(r, p, y);
+//  return tf2::toMsg(q);
+//}
+
+Cluster2D::Cluster2D(const int rows, const int cols, const float range)
+{
+  rows_ = rows;
+  cols_ = cols;
+  siz_ = rows * cols;
+  range_ = range;
+  scale_ = 0.5 * static_cast<float>(rows_) / range_;
+  inv_res_x_ = 0.5 * static_cast<float>(cols_) / range_;
+  inv_res_y_ = 0.5 * static_cast<float>(rows_) / range_;
+  point2grid_.clear();
+  id_img_.assign(siz_, -1);
+  pc_ptr_.reset();
+  valid_indices_in_pc_ = nullptr;
+}
+
+void Cluster2D::traverse(Node * x)
+{
+  std::vector<Node *> p;
+  p.clear();
+
+  while (x->traversed == 0) {
+    p.push_back(x);
+    x->traversed = 2;
+    x = x->center_node;
+  }
+  if (x->traversed == 2) {
+    for (int i = static_cast<int>(p.size()) - 1; i >= 0 && p[i] != x; i--) {
+      p[i]->is_center = true;
+    }
+    x->is_center = true;
+  }
+  for (size_t i = 0; i < p.size(); i++) {
+    Node * y = p[i];
+    y->traversed = 1;
+    y->parent = x->parent;
+  }
+}
+
+void Cluster2D::cluster(
+  const std::shared_ptr<float> & inferred_data, const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
+  const pcl::PointIndices & valid_indices, float objectness_thresh,
+  bool use_all_grids_for_clustering)
+{
+  const float * category_pt_data = inferred_data.get();
+  const float * instance_pt_x_data = inferred_data.get() + siz_;
+  const float * instance_pt_y_data = inferred_data.get() + siz_ * 2;
+
+  pc_ptr_ = pc_ptr;
+
+  std::vector<std::vector<Node>> nodes(rows_, std::vector<Node>(cols_, Node()));
+
+  valid_indices_in_pc_ = &(valid_indices.indices);
+  point2grid_.assign(valid_indices_in_pc_->size(), -1);
+
+  for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) {
+    int point_id = valid_indices_in_pc_->at(i);
+    const auto & point = pc_ptr_->points[point_id];
+    // * the coordinates of x and y have been exchanged in feature generation
+    // step,
+    // so we swap them back here.
+    int pos_x = F2I(point.y, range_, inv_res_x_);  // col
+    int pos_y = F2I(point.x, range_, inv_res_y_);  // row
+    if (IsValidRowCol(pos_y, pos_x)) {
+      point2grid_[i] = RowCol2Grid(pos_y, pos_x);
+      nodes[pos_y][pos_x].point_num++;
+    }
+  }
+
+  for (int row = 0; row < rows_; ++row) {
+    for (int col = 0; col < cols_; ++col) {
+      int grid = RowCol2Grid(row, col);
+      Node * node = &nodes[row][col];
+      DisjointSetMakeSet(node);
+      node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) &&
+                        (*(category_pt_data + grid) >= objectness_thresh);
+      int center_row = std::round(row + instance_pt_x_data[grid] * scale_);
+      int center_col = std::round(col + instance_pt_y_data[grid] * scale_);
+      center_row = std::min(std::max(center_row, 0), rows_ - 1);
+      center_col = std::min(std::max(center_col, 0), cols_ - 1);
+      node->center_node = &nodes[center_row][center_col];
+    }
+  }
+
+  for (int row = 0; row < rows_; ++row) {
+    for (int col = 0; col < cols_; ++col) {
+      Node * node = &nodes[row][col];
+      if (node->is_object && node->traversed == 0) {
+        traverse(node);
+      }
+    }
+  }
+
+  for (int row = 0; row < rows_; ++row) {
+    for (int col = 0; col < cols_; ++col) {
+      Node * node = &nodes[row][col];
+      if (!node->is_center) {
+        continue;
+      }
+      for (int row2 = row - 1; row2 <= row + 1; ++row2) {
+        for (int col2 = col - 1; col2 <= col + 1; ++col2) {
+          if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) {
+            Node * node2 = &nodes[row2][col2];
+            if (node2->is_center) {
+              DisjointSetUnion(node, node2);
+            }
+          }
+        }
+      }
+    }
+  }
+
+  int count_obstacles = 0;
+  obstacles_.clear();
+  id_img_.assign(siz_, -1);
+  for (int row = 0; row < rows_; ++row) {
+    for (int col = 0; col < cols_; ++col) {
+      Node * node = &nodes[row][col];
+      if (!node->is_object) {
+        continue;
+      }
+      Node * root = DisjointSetFind(node);
+      if (root->obstacle_id < 0) {
+        root->obstacle_id = count_obstacles++;
+        obstacles_.push_back(Obstacle());
+      }
+      int grid = RowCol2Grid(row, col);
+      id_img_[grid] = root->obstacle_id;
+      obstacles_[root->obstacle_id].grids.push_back(grid);
+    }
+  }
+  filter(inferred_data);
+  classify(inferred_data);
+}
+
+void Cluster2D::filter(const std::shared_ptr<float> & inferred_data)
+{
+  const float * confidence_pt_data = inferred_data.get() + siz_ * 3;
+  const float * heading_pt_x_data = inferred_data.get() + siz_ * 9;
+  const float * heading_pt_y_data = inferred_data.get() + siz_ * 10;
+  const float * height_pt_data = inferred_data.get() + siz_ * 11;
+
+  for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
+    Obstacle * obs = &obstacles_[obstacle_id];
+    double score = 0.0;
+    double height = 0.0;
+    double vec_x = 0.0;
+    double vec_y = 0.0;
+    for (int grid : obs->grids) {
+      score += static_cast<double>(confidence_pt_data[grid]);
+      height += static_cast<double>(height_pt_data[grid]);
+      vec_x += heading_pt_x_data[grid];
+      vec_y += heading_pt_y_data[grid];
+    }
+    obs->score = score / static_cast<double>(obs->grids.size());
+    obs->height = height / static_cast<double>(obs->grids.size());
+    obs->heading = std::atan2(vec_y, vec_x) * 0.5;
+    obs->cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
+  }
+}
+
+void Cluster2D::classify(const std::shared_ptr<float> & inferred_data)
+{
+  const float * classify_pt_data = inferred_data.get() + siz_ * 4;
+  int num_classes = 5;
+  for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) {
+    Obstacle * obs = &obstacles_[obs_id];
+
+    for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) {
+      int grid = obs->grids[grid_id];
+      for (int k = 0; k < num_classes; k++) {
+        obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid];
+      }
+    }
+    int meta_type_id = 0;
+    for (int k = 0; k < num_classes; k++) {
+      obs->meta_type_probs[k] /= obs->grids.size();
+      if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) {
+        meta_type_id = k;
+      }
+    }
+    obs->meta_type = static_cast<MetaType>(meta_type_id);
+  }
+}
+
+void Cluster2D::getObjectVector(const float confidence_thresh,
+                                const float height_thresh,
+                                const int min_pts_num,std::vector<Obstacle> & objvec)
+{
+//    CHECK(valid_indices_in_pc_ != nullptr);
+
+//    for (size_t i = 0; i < point2grid_.size(); ++i)
+//    {
+//        int grid = point2grid_[i];
+//        if (grid < 0)
+//        {
+//            continue;
+//        }
+
+//        CHECK_GE(grid, 0);
+//        CHECK_LT(grid, grids_);
+//        int obstacle_id = id_img_[grid];
+
+//        int point_id = valid_indices_in_pc_->at(i);
+//        CHECK_GE(point_id, 0);
+//        CHECK_LT(point_id, static_cast<int>(pc_ptr_->size()));
+
+//        if (obstacle_id >= 0 &&
+//            obstacles_[obstacle_id].score >= confidence_thresh)
+//        {
+////            if (height_thresh < 0 ||
+////                pc_ptr_->points[point_id].z <=
+////                obstacles_[obstacle_id].height + height_thresh)
+////            {
+//                obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]);
+// //           }
+//        }
+//    }
+
+//    for (size_t obstacle_id = 0; obstacle_id < obstacles_.size();
+//         obstacle_id++)
+//    {
+//        Obstacle *obs = &obstacles_[obstacle_id];
+//        if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num)
+//        {
+//            continue;
+//        }
+
+//        objvec.push_back(*obs);
+//    }
+
+}
+
+
+//tier4_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject(
+//  const Obstacle & in_obstacle, const std_msgs::msg::Header & in_header)
+//{
+//  using autoware_auto_perception_msgs::msg::DetectedObjectKinematics;
+//  using autoware_auto_perception_msgs::msg::ObjectClassification;
+
+//  tier4_perception_msgs::msg::DetectedObjectWithFeature resulting_object;
+//  // pcl::PointCloud<pcl::PointXYZI> in_cluster = *(in_obstacle.cloud_ptr);
+
+//  resulting_object.object.classification.emplace_back(
+//    autoware_auto_perception_msgs::build<ObjectClassification>()
+//      .label(ObjectClassification::UNKNOWN)
+//      .probability(in_obstacle.score));
+//  if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) {
+//    resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN;
+//  } else if (in_obstacle.meta_type == MetaType::META_NON_MOT) {
+//    resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE;
+//  } else if (in_obstacle.meta_type == MetaType::META_SMALL_MOT) {
+//    resulting_object.object.classification.front().label = ObjectClassification::CAR;
+//  } else if (in_obstacle.meta_type == MetaType::META_BIG_MOT) {
+//    resulting_object.object.classification.front().label = ObjectClassification::BUS;
+//  } else {
+//    // resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN;
+//    resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN;
+//  }
+
+//  pcl::PointXYZ min_point;
+//  pcl::PointXYZ max_point;
+//  for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end();
+//       ++pit) {
+//    if (pit->x < min_point.x) {
+//      min_point.x = pit->x;
+//    }
+//    if (pit->y < min_point.y) {
+//      min_point.y = pit->y;
+//    }
+//    if (pit->z < min_point.z) {
+//      min_point.z = pit->z;
+//    }
+//    if (pit->x > max_point.x) {
+//      max_point.x = pit->x;
+//    }
+//    if (pit->y > max_point.y) {
+//      max_point.y = pit->y;
+//    }
+//    if (pit->z > max_point.z) {
+//      max_point.z = pit->z;
+//    }
+//  }
+
+//  // cluster and ground filtering
+//  pcl::PointCloud<pcl::PointXYZI> cluster;
+//  const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f);
+//  for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end();
+//       ++pit) {
+//    if (min_height < pit->z) {
+//      cluster.points.push_back(*pit);
+//    }
+//  }
+//  min_point.z = 0.0;
+//  max_point.z = 0.0;
+//  for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) {
+//    if (pit->z < min_point.z) {
+//      min_point.z = pit->z;
+//    }
+//    if (pit->z > max_point.z) {
+//      max_point.z = pit->z;
+//    }
+//  }
+//  sensor_msgs::msg::PointCloud2 ros_pc;
+//  pcl::toROSMsg(cluster, ros_pc);
+//  resulting_object.feature.cluster = ros_pc;
+//  resulting_object.feature.cluster.header = in_header;
+
+//  // position
+//  const float height = max_point.z - min_point.z;
+//  const float length = max_point.x - min_point.x;
+//  const float width = max_point.y - min_point.y;
+//  resulting_object.object.kinematics.pose_with_covariance.pose.position.x =
+//    min_point.x + length / 2;
+//  resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2;
+//  resulting_object.object.kinematics.pose_with_covariance.pose.position.z =
+//    min_point.z + height / 2;
+
+//  resulting_object.object.kinematics.pose_with_covariance.pose.orientation =
+//    getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading);
+//  resulting_object.object.kinematics.orientation_availability =
+//    DetectedObjectKinematics::SIGN_UNKNOWN;
+
+//  return resulting_object;
+//}
+
+//void Cluster2D::getObjects(
+//  const float confidence_thresh, const float height_thresh, const int min_pts_num,
+//  tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects,
+//  const std_msgs::msg::Header & in_header)
+//{
+//  for (size_t i = 0; i < point2grid_.size(); ++i) {
+//    int grid = point2grid_[i];
+//    if (grid < 0) {
+//      continue;
+//    }
+
+//    int obstacle_id = id_img_[grid];
+
+//    int point_id = valid_indices_in_pc_->at(i);
+
+//    if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) {
+//      if (
+//        height_thresh < 0 ||
+//        pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) {
+//        obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]);
+//      }
+//    }
+//  }
+
+//  for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
+//    Obstacle * obs = &obstacles_[obstacle_id];
+//    if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num) {
+//      continue;
+//    }
+//    tier4_perception_msgs::msg::DetectedObjectWithFeature out_obj =
+//      obstacleToObject(*obs, in_header);
+//    objects.feature_objects.push_back(out_obj);
+//  }
+//  objects.header = in_header;
+//}

src/detection/detection_lidar_cnn_segmentation_trt/src/debugger.cpp

@@ -0,0 +1,106 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lidar_apollo_instance_segmentation/debugger.hpp"
+
+//#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+//#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+//#include <pcl_conversions/pcl_conversions.h>
+
+
+Debugger::Debugger()
+{
+
+}
+
+//Debugger::Debugger(rclcpp::Node * node)
+//{
+//  instance_pointcloud_pub_ =
+//    node->create_publisher<sensor_msgs::msg::PointCloud2>("debug/instance_pointcloud", 1);
+//}
+
+//void Debugger::publishColoredPointCloud(
+//  const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input)
+//{
+//  using autoware_auto_perception_msgs::msg::ObjectClassification;
+//  pcl::PointCloud<pcl::PointXYZRGB> colored_pointcloud;
+//  for (size_t i = 0; i < input.feature_objects.size(); i++) {
+//    pcl::PointCloud<pcl::PointXYZI> object_pointcloud;
+//    pcl::fromROSMsg(input.feature_objects.at(i).feature.cluster, object_pointcloud);
+
+//    int red = 0, green = 0, blue = 0;
+//    switch (input.feature_objects.at(i).object.classification.front().label) {
+//      case ObjectClassification::CAR: {
+//        red = 255;
+//        green = 0;
+//        blue = 0;
+//        break;
+//      }
+//      case ObjectClassification::TRUCK: {
+//        red = 255;
+//        green = 127;
+//        blue = 0;
+//        break;
+//      }
+//      case ObjectClassification::BUS: {
+//        red = 255;
+//        green = 0;
+//        blue = 127;
+//        break;
+//      }
+//      case ObjectClassification::PEDESTRIAN: {
+//        red = 0;
+//        green = 255;
+//        blue = 0;
+//        break;
+//      }
+//      case ObjectClassification::BICYCLE: {
+//        red = 0;
+//        green = 0;
+//        blue = 255;
+//        break;
+//      }
+//      case ObjectClassification::MOTORCYCLE: {
+//        red = 0;
+//        green = 127;
+//        blue = 255;
+//        break;
+//      }
+//      case ObjectClassification::UNKNOWN: {
+//        red = 255;
+//        green = 255;
+//        blue = 255;
+//        break;
+//      }
+//    }
+
+//    for (size_t i = 0; i < object_pointcloud.size(); i++) {
+//      pcl::PointXYZRGB colored_point;
+//      colored_point.x = object_pointcloud[i].x;
+//      colored_point.y = object_pointcloud[i].y;
+//      colored_point.z = object_pointcloud[i].z;
+//      colored_point.r = red;
+//      colored_point.g = green;
+//      colored_point.b = blue;
+//      colored_pointcloud.push_back(colored_point);
+//    }
+//  }
+//  sensor_msgs::msg::PointCloud2 output_msg;
+//  pcl::toROSMsg(colored_pointcloud, output_msg);
+//  output_msg.header = input.header;
+//  instance_pointcloud_pub_->publish(output_msg);
+//}

src/detection/detection_lidar_cnn_segmentation_trt/src/detector.cpp

@@ -0,0 +1,220 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lidar_apollo_instance_segmentation/detector.hpp"
+
+#include "lidar_apollo_instance_segmentation/feature_map.hpp"
+
+#include <NvCaffeParser.h>
+#include <NvInfer.h>
+//#include <pcl_conversions/pcl_conversions.h>
+
+LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation()
+{
+  int range, width, height;
+  bool use_intensity_feature, use_constant_feature;
+  std::string engine_file;
+  std::string prototxt_file;
+  std::string caffemodel_file;
+  score_threshold_ = 0.8;//node_->declare_parameter("score_threshold", 0.8);
+  range = 60;//node_->declare_parameter("range", 60);
+  width = 640;//node_->declare_parameter("width", 640);
+  height = 640;//node_->declare_parameter("height", 640);
+  engine_file =  "deploy.engine";//node_->declare_parameter("engine_file", "vls-128.engine");
+  prototxt_file = "hdl-64.prototxt";//node_->declare_parameter("prototxt_file", "vls-128.prototxt");
+  caffemodel_file = "deploy.caffemodel";// node_->declare_parameter("caffemodel_file", "vls-128.caffemodel");
+  use_intensity_feature = true;//node_->declare_parameter("use_intensity_feature", true);
+  use_constant_feature = true;//node_->declare_parameter("use_constant_feature", true);
+  target_frame_ = "base_link";//node_->declare_parameter("target_frame", "base_link");
+  z_offset_ =  -2.0;//node_->declare_parameter<float>("z_offset", -2.0);
+
+  // load weight file
+  std::ifstream fs(engine_file);
+  if (!fs.is_open()) {
+      std::cout<<"Could not find engine file. try making TensorRT engine from caffemodel and prototxt"<<std::endl;
+//    RCLCPP_INFO(
+//      node_->get_logger(),
+//      "Could not find %s. try making TensorRT engine from caffemodel and prototxt",
+//      engine_file.c_str());
+    Tn::Logger logger;
+    nvinfer1::IBuilder * builder = nvinfer1::createInferBuilder(logger);
+    nvinfer1::INetworkDefinition * network = builder->createNetworkV2(0U);
+    nvcaffeparser1::ICaffeParser * parser = nvcaffeparser1::createCaffeParser();
+    nvinfer1::IBuilderConfig * config = builder->createBuilderConfig();
+    const nvcaffeparser1::IBlobNameToTensor * blob_name2tensor = parser->parse(
+      prototxt_file.c_str(), caffemodel_file.c_str(), *network, nvinfer1::DataType::kFLOAT);
+    std::string output_node = "deconv0";
+    auto output = blob_name2tensor->find(output_node.c_str());
+    if (output == nullptr) {
+        std::cout<<"can not find output node."<<std::endl;
+//      RCLCPP_ERROR(node_->get_logger(), "can not find output named %s", output_node.c_str());
+    }
+    network->markOutput(*output);
+#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400
+    config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, 1 << 30);
+#else
+    config->setMaxWorkspaceSize(1 << 30);
+#endif
+    nvinfer1::IHostMemory * plan = builder->buildSerializedNetwork(*network, *config);
+    assert(plan != nullptr);
+    std::ofstream outfile(engine_file, std::ofstream::binary);
+    assert(!outfile.fail());
+    outfile.write(reinterpret_cast<char *>(plan->data()), plan->size());
+    outfile.close();
+    if (network) {
+      delete network;
+    }
+    if (parser) {
+      delete parser;
+    }
+    if (builder) {
+      delete builder;
+    }
+    if (config) {
+      delete config;
+    }
+    if (plan) {
+      delete plan;
+    }
+    std::cout<<"convert engine success."<<std::endl;
+  }
+  net_ptr_.reset(new Tn::trtNet(engine_file));
+
+  // feature map generator: pre process
+  feature_generator_ = std::make_shared<FeatureGenerator>(
+    width, height, range, use_intensity_feature, use_constant_feature);
+
+  // cluster: post process
+  cluster2d_ = std::make_shared<Cluster2D>(width, height, range);
+
+
+}
+
+
+bool LidarApolloInstanceSegmentation::detectDynamicObjects(
+   const pcl::PointCloud<pcl::PointXYZI>::Ptr &pcl_pointcloud_raw_ptr,
+   std::vector<Obstacle> & objvec)
+{
+    // generate feature map
+    std::shared_ptr<FeatureMapInterface> feature_map_ptr =
+      feature_generator_->generate(pcl_pointcloud_raw_ptr);
+
+    // inference
+    std::shared_ptr<float> inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]);
+    net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get());
+
+    // post process
+    const float objectness_thresh = 0.5;
+    pcl::PointIndices valid_idx;
+    valid_idx.indices.resize(pcl_pointcloud_raw_ptr->size());
+    std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0);
+    cluster2d_->cluster(
+      inferred_data, pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh,
+      true /*use all grids for clustering*/);
+    const float height_thresh = 0.5;
+    const int min_pts_num = 3;
+//    cluster2d_->getObjects(
+//      score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header);
+
+//    // move down pointcloud z_offset in z axis
+//    for (std::size_t i = 0; i < output.feature_objects.size(); i++) {
+//      sensor_msgs::msg::PointCloud2 transformed_cloud;
+//      transformCloud(output.feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_);
+//      output.feature_objects.at(i).feature.cluster = transformed_cloud;
+//    }
+
+//    output.header = transformed_cloud.header;
+    return true;
+}
+//bool LidarApolloInstanceSegmentation::transformCloud(
+//  const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud,
+//  float z_offset)
+//{
+//  // TODO(mitsudome-r): remove conversion once pcl_ros transform are available.
+//  pcl::PointCloud<pcl::PointXYZI> pcl_input, pcl_transformed_cloud;
+//  pcl::fromROSMsg(input, pcl_input);
+
+//  // transform pointcloud to target_frame
+//  if (target_frame_ != input.header.frame_id) {
+//    try {
+//      geometry_msgs::msg::TransformStamped transform_stamped;
+//      transform_stamped = tf_buffer_.lookupTransform(
+//        target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500));
+//      Eigen::Matrix4f affine_matrix =
+//        tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+//      pcl::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix);
+//      transformed_cloud.header.frame_id = target_frame_;
+//      pcl_transformed_cloud.header.frame_id = target_frame_;
+//    } catch (tf2::TransformException & ex) {
+//      RCLCPP_WARN(node_->get_logger(), "%s", ex.what());
+//      return false;
+//    }
+//  } else {
+//    pcl_transformed_cloud = pcl_input;
+//  }
+
+//  // move pointcloud z_offset in z axis
+//  pcl::PointCloud<pcl::PointXYZI> pointcloud_with_z_offset;
+//  Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset));
+//  Eigen::Matrix4f z_up_transform = z_up_translation.matrix();
+//  pcl::transformPointCloud(pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform);
+
+//  pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud);
+
+//  return true;
+//}
+
+//bool LidarApolloInstanceSegmentation::detectDynamicObjects(
+//  const sensor_msgs::msg::PointCloud2 & input,
+//  tier4_perception_msgs::msg::DetectedObjectsWithFeature & output)
+//{
+//  // move up pointcloud z_offset in z axis
+//  sensor_msgs::msg::PointCloud2 transformed_cloud;
+//  transformCloud(input, transformed_cloud, z_offset_);
+
+//  // convert from ros to pcl
+//  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud<pcl::PointXYZI>);
+//  pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr);
+
+//  // generate feature map
+//  std::shared_ptr<FeatureMapInterface> feature_map_ptr =
+//    feature_generator_->generate(pcl_pointcloud_raw_ptr);
+
+//  // inference
+//  std::shared_ptr<float> inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]);
+//  net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get());
+
+//  // post process
+//  const float objectness_thresh = 0.5;
+//  pcl::PointIndices valid_idx;
+//  valid_idx.indices.resize(pcl_pointcloud_raw_ptr->size());
+//  std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0);
+//  cluster2d_->cluster(
+//    inferred_data, pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh,
+//    true /*use all grids for clustering*/);
+//  const float height_thresh = 0.5;
+//  const int min_pts_num = 3;
+//  cluster2d_->getObjects(
+//    score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header);
+
+//  // move down pointcloud z_offset in z axis
+//  for (std::size_t i = 0; i < output.feature_objects.size(); i++) {
+//    sensor_msgs::msg::PointCloud2 transformed_cloud;
+//    transformCloud(output.feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_);
+//    output.feature_objects.at(i).feature.cluster = transformed_cloud;
+//  }
+
+//  output.header = transformed_cloud.header;
+//  return true;
+//}

src/detection/detection_lidar_cnn_segmentation_trt/src/feature_generator.cpp

@@ -0,0 +1,95 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lidar_apollo_instance_segmentation/feature_generator.hpp"
+
+#include "lidar_apollo_instance_segmentation/log_table.hpp"
+
+namespace
+{
+inline float normalizeIntensity(float intensity) { return intensity / 255.0f; }
+}  // namespace
+
+FeatureGenerator::FeatureGenerator(
+  const int width, const int height, const int range, const bool use_intensity_feature,
+  const bool use_constant_feature)
+: min_height_(-5.0),
+  max_height_(5.0),
+  use_intensity_feature_(use_intensity_feature),
+  use_constant_feature_(use_constant_feature)
+{
+  // select feature map type
+  if (use_constant_feature && use_intensity_feature) {
+    map_ptr_ = std::make_shared<FeatureMapWithConstantAndIntensity>(width, height, range);
+  } else if (use_constant_feature) {
+    map_ptr_ = std::make_shared<FeatureMapWithConstant>(width, height, range);
+  } else if (use_intensity_feature) {
+    map_ptr_ = std::make_shared<FeatureMapWithIntensity>(width, height, range);
+  } else {
+    map_ptr_ = std::make_shared<FeatureMap>(width, height, range);
+  }
+  map_ptr_->initializeMap(map_ptr_->map_data);
+}
+
+std::shared_ptr<FeatureMapInterface> FeatureGenerator::generate(
+  const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr)
+{
+  const double epsilon = 1e-6;
+  map_ptr_->resetMap(map_ptr_->map_data);
+
+  int size = map_ptr_->height * map_ptr_->width;
+
+  const float inv_res_x = 0.5 * map_ptr_->width / map_ptr_->range;
+  const float inv_res_y = 0.5 * map_ptr_->height / map_ptr_->range;
+
+  for (size_t i = 0; i < pc_ptr->points.size(); ++i) {
+    if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) {
+      continue;
+    }
+
+    const int pos_x = std::floor((map_ptr_->range - pc_ptr->points[i].y) * inv_res_x);  // x on grid
+    const int pos_y = std::floor((map_ptr_->range - pc_ptr->points[i].x) * inv_res_y);  // y on grid
+    if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) {
+      continue;
+    }
+
+    const int idx = pos_y * map_ptr_->width + pos_x;
+
+    if (map_ptr_->max_height_data[idx] < pc_ptr->points[i].z) {
+      map_ptr_->max_height_data[idx] = pc_ptr->points[i].z;
+      if (map_ptr_->top_intensity_data != nullptr) {
+        map_ptr_->top_intensity_data[idx] = normalizeIntensity(pc_ptr->points[i].intensity);
+      }
+    }
+    map_ptr_->mean_height_data[idx] += static_cast<float>(pc_ptr->points[i].z);
+    if (map_ptr_->mean_intensity_data != nullptr) {
+      map_ptr_->mean_intensity_data[idx] += normalizeIntensity(pc_ptr->points[i].intensity);
+    }
+    map_ptr_->count_data[idx] += 1.0f;
+  }
+
+  for (int i = 0; i < size; ++i) {
+    if (map_ptr_->count_data[i] < epsilon) {
+      map_ptr_->max_height_data[i] = 0.0f;
+    } else {
+      map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i];
+      if (map_ptr_->mean_intensity_data != nullptr) {
+        map_ptr_->mean_intensity_data[i] /= map_ptr_->count_data[i];
+      }
+      map_ptr_->nonempty_data[i] = 1.0f;
+    }
+    map_ptr_->count_data[i] = calcApproximateLog(map_ptr_->count_data[i]);
+  }
+  return map_ptr_;
+}

src/detection/detection_lidar_cnn_segmentation_trt/src/feature_map.cpp

@@ -0,0 +1,164 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lidar_apollo_instance_segmentation/feature_map.hpp"
+
+#include "lidar_apollo_instance_segmentation/util.hpp"
+
+#include <cmath>
+
+FeatureMapInterface::FeatureMapInterface(
+  const int _channels, const int _width, const int _height, const int _range)
+: channels(_channels),
+  width(_width),
+  height(_height),
+  range(_range),
+  max_height_data(nullptr),
+  mean_height_data(nullptr),
+  count_data(nullptr),
+  direction_data(nullptr),
+  top_intensity_data(nullptr),
+  mean_intensity_data(nullptr),
+  distance_data(nullptr),
+  nonempty_data(nullptr)
+{
+  map_data.resize(width * height * channels);
+}
+
+FeatureMap::FeatureMap(const int width, const int height, const int range)
+: FeatureMapInterface(4, width, height, range)
+{
+  max_height_data = &(map_data[0]) + width * height * 0;
+  mean_height_data = &(map_data[0]) + width * height * 1;
+  count_data = &(map_data[0]) + width * height * 2;
+  nonempty_data = &(map_data[0]) + width * height * 3;
+}
+void FeatureMap::initializeMap([[maybe_unused]] std::vector<float> & map) {}
+void FeatureMap::resetMap([[maybe_unused]] std::vector<float> & map)
+{
+  const int size = width * height;
+  for (int i = 0; i < size; ++i) {
+    max_height_data[i] = -5.0f;
+    mean_height_data[i] = 0.0f;
+    count_data[i] = 0.0f;
+    nonempty_data[i] = 0.0f;
+  }
+}
+
+FeatureMapWithIntensity::FeatureMapWithIntensity(const int width, const int height, const int range)
+: FeatureMapInterface(6, width, height, range)
+{
+  max_height_data = &(map_data[0]) + width * height * 0;
+  mean_height_data = &(map_data[0]) + width * height * 1;
+  count_data = &(map_data[0]) + width * height * 2;
+  top_intensity_data = &(map_data[0]) + width * height * 3;
+  mean_intensity_data = &(map_data[0]) + width * height * 4;
+  nonempty_data = &(map_data[0]) + width * height * 5;
+}
+void FeatureMapWithIntensity::initializeMap([[maybe_unused]] std::vector<float> & map) {}
+void FeatureMapWithIntensity::resetMap([[maybe_unused]] std::vector<float> & map)
+{
+  const int size = width * height;
+  for (int i = 0; i < size; ++i) {
+    max_height_data[i] = -5.0f;
+    mean_height_data[i] = 0.0f;
+    count_data[i] = 0.0f;
+    top_intensity_data[i] = 0.0f;
+    mean_intensity_data[i] = 0.0f;
+    nonempty_data[i] = 0.0f;
+  }
+}
+
+FeatureMapWithConstant::FeatureMapWithConstant(const int width, const int height, const int range)
+: FeatureMapInterface(6, width, height, range)
+{
+  max_height_data = &(map_data[0]) + width * height * 0;
+  mean_height_data = &(map_data[0]) + width * height * 1;
+  count_data = &(map_data[0]) + width * height * 2;
+  direction_data = &(map_data[0]) + width * height * 3;
+  distance_data = &(map_data[0]) + width * height * 4;
+  nonempty_data = &(map_data[0]) + width * height * 5;
+}
+void FeatureMapWithConstant::initializeMap([[maybe_unused]] std::vector<float> & map)
+{
+  for (int row = 0; row < height; ++row) {
+    for (int col = 0; col < width; ++col) {
+      int idx = row * width + col;
+      // * row <-> x, column <-> y
+      // return the distance from my car to center of the grid.
+      // Pc means point cloud = real world scale. so transform pixel scale to
+      // real world scale
+      float center_x = Pixel2Pc(row, height, range);
+      float center_y = Pixel2Pc(col, width, range);
+      // normalization. -0.5~0.5
+      direction_data[idx] = static_cast<float>(std::atan2(center_y, center_x) / (2.0 * M_PI));
+      distance_data[idx] = static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
+    }
+  }
+}
+
+void FeatureMapWithConstant::resetMap([[maybe_unused]] std::vector<float> & map)
+{
+  const int size = width * height;
+  for (int i = 0; i < size; ++i) {
+    max_height_data[i] = -5.0f;
+    mean_height_data[i] = 0.0f;
+    count_data[i] = 0.0f;
+    nonempty_data[i] = 0.0f;
+  }
+}
+
+FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity(
+  const int width, const int height, const int range)
+: FeatureMapInterface(8, width, height, range)
+{
+  max_height_data = &(map_data[0]) + width * height * 0;
+  mean_height_data = &(map_data[0]) + width * height * 1;
+  count_data = &(map_data[0]) + width * height * 2;
+  direction_data = &(map_data[0]) + width * height * 3;
+  top_intensity_data = &(map_data[0]) + width * height * 4;
+  mean_intensity_data = &(map_data[0]) + width * height * 5;
+  distance_data = &(map_data[0]) + width * height * 6;
+  nonempty_data = &(map_data[0]) + width * height * 7;
+}
+void FeatureMapWithConstantAndIntensity::initializeMap([[maybe_unused]] std::vector<float> & map)
+{
+  for (int row = 0; row < height; ++row) {
+    for (int col = 0; col < width; ++col) {
+      int idx = row * width + col;
+      // * row <-> x, column <-> y
+      // return the distance from my car to center of the grid.
+      // Pc means point cloud = real world scale. so transform pixel scale to
+      // real world scale
+      float center_x = Pixel2Pc(row, height, range);
+      float center_y = Pixel2Pc(col, width, range);
+      // normalization. -0.5~0.5
+      direction_data[idx] = static_cast<float>(std::atan2(center_y, center_x) / (2.0 * M_PI));
+      distance_data[idx] = static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
+    }
+  }
+}
+
+void FeatureMapWithConstantAndIntensity::resetMap([[maybe_unused]] std::vector<float> & map)
+{
+  const int size = width * height;
+  for (int i = 0; i < size; ++i) {
+    max_height_data[i] = -5.0f;
+    mean_height_data[i] = 0.0f;
+    count_data[i] = 0.0f;
+    top_intensity_data[i] = 0.0f;
+    mean_intensity_data[i] = 0.0f;
+    nonempty_data[i] = 0.0f;
+  }
+}

src/detection/detection_lidar_cnn_segmentation_trt/src/log_table.cpp

@@ -0,0 +1,45 @@
+// Copyright 2020 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "lidar_apollo_instance_segmentation/log_table.hpp"
+
+#include <cmath>
+#include <string>
+#include <vector>
+
+namespace
+{
+struct LogTable
+{
+  std::vector<float> data;
+  LogTable()
+  {
+    data.resize(256 * 10);
+    for (size_t i = 0; i < data.size(); ++i) {
+      data[i] = std::log1p(static_cast<float>(i / 10.0));
+    }
+  }
+};
+}  // namespace
+
+static ::LogTable log_table;
+
+float calcApproximateLog(float num)
+{
+  int integer_num = static_cast<int>(num * 10.0);
+  if (integer_num < static_cast<int>(log_table.data.size())) {
+    return log_table.data[integer_num];
+  }
+  return std::log(static_cast<float>(1.0 + num));
+}