├── src
    ├── tensorrt_bcnn_main.cpp
    ├── feature_generator.cpp
    ├── tensorrt_bcnn_ros.cpp
    └── cluster2d.cpp
├── README.md
├── include
    ├── data_reader.h
    ├── feature_generator.h
    ├── disjoint_set.h
    ├── tensorrt_bcnn_ros.h
    ├── util.h
    └── cluster2d.h
├── package.xml
├── lib
    ├── include
    │   ├── UpsampleLayer.h
    │   ├── TrtNet.h
    │   ├── Utils.h
    │   └── PluginFactory.h
    └── src
    │   ├── UpsampleLayer.cpp
    │   ├── UpsampleLayer.cu
    │   └── TrtNet.cpp
├── launch
    └── tensorrt_bcnn.launch
└── CMakeLists.txt


/src/tensorrt_bcnn_main.cpp:
--------------------------------------------------------------------------------
 1 | #include "tensorrt_bcnn_ros.h"
 2 | 
 3 | int main(int argc, char** argv) {
 4 |   ros::init(argc, argv, "tensorrt_bcnn");
 5 |   TensorrtBcnnROS node;
 6 |   node.init();
 7 |   node.createROSPubSub();
 8 |   ros::spin();
 9 | 
10 |   return 0;
11 | }
12 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ### tensorrt_bcnn
 2 | 
 3 | Release version is https://github.com/tier4/Pilot.Auto/tree/master/perception/object_recognition/detection/lidar_apollo_instance_segmentation  
 4 | 
 5 | ![result](https://github.com/kosuke55/tensorrt_bcnn/blob/media/media/bcnn_trt_pretrained_0208.gif)  
 6 | 
 7 | #### reference
 8 | [apollo 3D Obstacle Percption description][1]  
 9 | 
10 | [1]:https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md
11 | 
12 | [autoware_perception description][2]  
13 | 
14 | [2]:https://github.com/k0suke-murakami/autoware_perception/tree/feature/integration_baidu_seg/lidar_apollo_cnn_seg_detect
15 | 


--------------------------------------------------------------------------------
/include/data_reader.h:
--------------------------------------------------------------------------------
 1 | #ifndef _DATA_READER_H_
 2 | #define _DATA_READER_H_
 3 | 
 4 | #include <list>
 5 | #include <string>
 6 | #include <vector>
 7 | 
 8 | namespace Tn {
 9 | std::list<std::string> readFileList(const std::string& fileName);
10 | 
11 | struct Source {
12 |   std::string fileName;
13 |   int label;
14 | };
15 | std::list<Source> readLabelFileList(const std::string& fileName);
16 | 
17 | struct Bbox {
18 |   int classId;
19 |   int left;
20 |   int right;
21 |   int top;
22 |   int bot;
23 |   float score;
24 | };
25 | 
26 | std::tuple<std::list<std::string>, std::list<std::vector<Bbox>>>
27 | readObjectLabelFileList(const std::string& fileName);
28 | }
29 | 
30 | #endif
31 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |     <name>tensorrt_bcnn</name>
 4 |     <version>1.11.0</version>
 5 |     <description>tensorrt_bcnn</description>
 6 | 
 7 |     <maintainer email="kosuke.tnp@gmail.com">Kosuke Takeuchi</maintainer>
 8 |     <license>Apache 2.0</license>
 9 | 
10 |     <buildtool_depend>catkin</buildtool_depend>
11 | 
12 |     <build_depend>roscpp</build_depend>
13 |     <build_depend>roslib</build_depend>
14 |     <build_depend>cv_bridge</build_depend>
15 |     <build_depend>pcl_ros</build_depend>
16 |     <build_depend>autoware_perception_msgs</build_depend>
17 | 
18 |     <run_depend>roscpp</run_depend>
19 |     <run_depend>roslib</run_depend>
20 |     <run_depend>cv_bridge</run_depend>
21 |     <run_depend>pcl_ros</run_depend>
22 |     <run_depend>autoware_perception_msgs</run_depend>
23 | 
24 |     <export></export>
25 | </package>
26 | 


--------------------------------------------------------------------------------
/include/feature_generator.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 3 |  *
 4 |  * Licensed under the Apache License, Version 2.0 (the "License");
 5 |  * you may not use this file except in compliance with the License.
 6 |  * You may obtain a copy of the License at
 7 |  *
 8 |  *     http://www.apache.org/licenses/LICENSE-2.0
 9 |  *
10 |  * Unless required by applicable law or agreed to in writing, software
11 |  * distributed under the License is distributed on an "AS IS" BASIS,
12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 |  * See the License for the specific language governing permissions and
14 |  * limitations under the License.
15 |  */
16 | #ifndef FEATURE_GENERATOR_H
17 | #define FEATURE_GENERATOR_H
18 | 
19 | #include <pcl/point_types.h>
20 | #include <pcl_ros/point_cloud.h>
21 | #include <opencv2/core/core.hpp>
22 | 
23 | #include "util.h"
24 | 
25 | class FeatureGenerator {
26 |  private:
27 |   int width_ = 640;
28 |   int height_ = 640;
29 |   int range_ = 0;
30 | 
31 |   float min_height_ = 0.0;
32 |   float max_height_ = 0.0;
33 | 
34 |   std::vector<float> log_table_;
35 | 
36 |   std::vector<int> map_idx_;
37 | 
38 |   float logCount(int count);
39 | 
40 |  public:
41 |   FeatureGenerator() {}
42 |   ~FeatureGenerator() {}
43 | 
44 |   bool init(float *in_feature_ptr, int range, int width, int height,
45 |             const bool use_constant_feature, const bool use_intensity_feature);
46 | 
47 |   void generate(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
48 |                 float *max_height_data,
49 |                 const bool use_constant_feature,
50 |                 const bool use_intensity_feature);
51 | };
52 | 
53 | #endif  // FEATURE_GENERATOR_H
54 | 


--------------------------------------------------------------------------------
/include/disjoint_set.h:
--------------------------------------------------------------------------------
 1 | /******************************************************************************
 2 |  * Copyright 2017 The Apollo Authors. All Rights Reserved.
 3 |  *
 4 |  * Licensed under the Apache License, Version 2.0 (the "License");
 5 |  * you may not use this file except in compliance with the License.
 6 |  * You may obtain a copy of the License at
 7 |  *
 8 |  * http://www.apache.org/licenses/LICENSE-2.0
 9 |  *
10 |  * Unless required by applicable law or agreed to in writing, software
11 |  * distributed under the License is distributed on an "AS IS" BASIS,
12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 |  * See the License for the specific language governing permissions and
14 |  * limitations under the License.
15 |  *****************************************************************************/
16 | 
17 | #ifndef MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
18 | #define MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
19 | 
20 | template <class T>
21 | void DisjointSetMakeSet(T *x) {
22 |   x->parent = x;
23 |   x->node_rank = 0;
24 | }
25 | 
26 | template <class T>
27 | T *DisjointSetFindRecursive(T *x) {
28 |   if (x->parent != x) {
29 |     x->parent = DisjointSetFindRecursive(x->parent);
30 |   }
31 |   return x->parent;
32 | }
33 | 
34 | template <class T>
35 | T *DisjointSetFind(T *x) {
36 |   T *y = x->parent;
37 |   if (y == x || y->parent == y) {
38 |     return y;
39 |   }
40 |   T *root = DisjointSetFindRecursive(y->parent);
41 |   x->parent = root;
42 |   y->parent = root;
43 |   return root;
44 | }
45 | 
46 | template <class T>
47 | void DisjointSetMerge(T *x, const T *y) {}
48 | 
49 | template <class T>
50 | void DisjointSetUnion(T *x, T *y) {
51 |   x = DisjointSetFind(x);
52 |   y = DisjointSetFind(y);
53 |   if (x == y) {
54 |     return;
55 |   }
56 |   if (x->node_rank < y->node_rank) {
57 |     x->parent = y;
58 |     DisjointSetMerge(y, x);
59 |   } else if (y->node_rank < x->node_rank) {
60 |     y->parent = x;
61 |     DisjointSetMerge(x, y);
62 |   } else {
63 |     y->parent = x;
64 |     x->node_rank++;
65 |     DisjointSetMerge(x, y);
66 |   }
67 | }
68 | 
69 | #endif  // MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_
70 | 


--------------------------------------------------------------------------------
/lib/include/UpsampleLayer.h:
--------------------------------------------------------------------------------
 1 | #ifndef _UPSAMPLE_LAYER_H
 2 | #define _UPSAMPLE_LAYER_H
 3 | 
 4 | #include <assert.h>
 5 | #include <cublas_v2.h>
 6 | #include <cudnn.h>
 7 | #include <string.h>
 8 | #include <cmath>
 9 | #include <iostream>
10 | #include "NvInfer.h"
11 | #include "Utils.h"
12 | 
13 | namespace nvinfer1 {
14 | class UpsampleLayerPlugin : public IPluginExt {
15 |  public:
16 |   explicit UpsampleLayerPlugin(const float scale, const int cudaThread = 512);
17 |   // create the plugin at runtime from a byte stream
18 |   UpsampleLayerPlugin(const void* data, size_t length);
19 | 
20 |   ~UpsampleLayerPlugin();
21 | 
22 |   int getNbOutputs() const override { return 1; }
23 | 
24 |   Dims getOutputDimensions(int index, const Dims* inputs,
25 |                            int nbInputDims) override;
26 | 
27 |   bool supportsFormat(DataType type, PluginFormat format) const override {
28 |     // std::cout << "supportsFormat=== type:"  << int(type) << "format" <<
29 |     // int(format) << std::endl;
30 |     return (type == DataType::kFLOAT || type == DataType::kHALF ||
31 |             type == DataType::kINT8) &&
32 |            format == PluginFormat::kNCHW;
33 |   }
34 | 
35 |   void configureWithFormat(const Dims* inputDims, int nbInputs,
36 |                            const Dims* outputDims, int nbOutputs, DataType type,
37 |                            PluginFormat format, int maxBatchSize) override;
38 | 
39 |   int initialize() override;
40 | 
41 |   virtual void terminate() override{};
42 | 
43 |   virtual size_t getWorkspaceSize(int maxBatchSize) const override { return 0; }
44 | 
45 |   virtual int enqueue(int batchSize, const void* const* inputs, void** outputs,
46 |                       void* workspace, cudaStream_t stream) override;
47 | 
48 |   virtual size_t getSerializationSize() override {
49 |     return sizeof(nvinfer1::Dims) + sizeof(mDataType) + sizeof(mScale) +
50 |            sizeof(mOutputWidth) + sizeof(mOutputHeight) + sizeof(mThreadCount);
51 |   }
52 | 
53 |   virtual void serialize(void* buffer) override;
54 | 
55 |   template <typename Dtype>
56 |   void forwardGpu(const Dtype* input, Dtype* outputint, int N, int C, int H,
57 |                   int W);
58 | 
59 |  private:
60 |   nvinfer1::Dims mCHW;
61 |   DataType mDataType{DataType::kFLOAT};
62 |   float mScale;
63 |   int mOutputWidth;
64 |   int mOutputHeight;
65 |   int mThreadCount;
66 | 
67 |   void* mInputBuffer{nullptr};
68 |   void* mOutputBuffer{nullptr};
69 | };
70 | };
71 | 
72 | #endif
73 | 


--------------------------------------------------------------------------------
/include/tensorrt_bcnn_ros.h:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | 
 3 | #include <cv_bridge/cv_bridge.h>
 4 | #include <nav_msgs/GetMap.h>
 5 | #include <sensor_msgs/Image.h>
 6 | #include <sensor_msgs/image_encodings.h>
 7 | 
 8 | #include <autoware_perception_msgs/DynamicObjectWithFeatureArray.h>
 9 | 
10 | // STL
11 | #include <chrono>
12 | #include <memory>
13 | #include <string>
14 | #include <vector>
15 | 
16 | // local
17 | #include "TrtNet.h"
18 | #include "cluster2d.h"
19 | #include "data_reader.h"
20 | #include "feature_generator.h"
21 | 
22 | #define __APP_NAME__ "tensorrt_bcnn"
23 | 
24 | class TensorrtBcnnROS {
25 |  private:
26 |   ros::NodeHandle nh_;
27 |   ros::NodeHandle pnh_;
28 |   ros::Subscriber sub_image_;
29 |   ros::Subscriber sub_points_;
30 |   ros::Publisher pub_confidence_image_;
31 |   ros::Publisher category_image_pub_;
32 |   ros::Publisher confidence_image_pub_;
33 |   ros::Publisher confidence_map_pub_;
34 |   ros::Publisher class_image_pub_;
35 |   ros::Publisher points_pub_;
36 |   ros::Publisher objects_pub_;
37 |   ros::Publisher d_objects_pub_;
38 | 
39 |   std::unique_ptr<Tn::trtNet> net_ptr_;
40 |   std::shared_ptr<Cluster2D> cluster2d_;
41 |   std::string topic_src_;
42 |   std::string trained_model_name_;
43 |   float score_threshold_;
44 | 
45 |   void imageCallback(const sensor_msgs::Image::ConstPtr &in_image);
46 |   void pointsCallback(const sensor_msgs::PointCloud2 &msg);
47 |   void reset_in_feature();
48 |   void pubColoredPoints(const autoware_perception_msgs::DynamicObjectWithFeatureArray &objects);
49 |   void convertDetected2Dynamic(
50 |       const autoware_perception_msgs::DynamicObjectWithFeatureArray &objects,
51 |       autoware_perception_msgs::DynamicObjectWithFeatureArray &d_objects);
52 |   cv::Mat get_heatmap(const float *feature);
53 |   cv::Mat get_confidence_image(const float *output);
54 |   cv::Mat get_class_image(const float *output);
55 |   nav_msgs::OccupancyGrid get_confidence_map(cv::Mat confidence_image);
56 | 
57 |   std::shared_ptr<FeatureGenerator> feature_generator_;
58 |   std_msgs::Header message_header_;
59 |   int range_;
60 |   int rows_;
61 |   int cols_;
62 |   int siz_;
63 |   int channels_ = 4;
64 |   bool use_intensity_feature_;
65 |   bool use_constant_feature_;
66 |   bool viz_category_image_;
67 |   bool viz_confidence_image_;
68 |   bool viz_class_image_;
69 |   bool pub_colored_points_;
70 | 
71 |   std::vector<float> in_feature;
72 | 
73 |  public:
74 |   TensorrtBcnnROS(/* args */);
75 |   ~TensorrtBcnnROS();
76 |   bool init();
77 | 
78 |   void createROSPubSub();
79 | };
80 | 


--------------------------------------------------------------------------------
/include/util.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 3 |  *
 4 |  * Licensed under the Apache License, Version 2.0 (the "License");
 5 |  * you may not use this file except in compliance with the License.
 6 |  * You may obtain a copy of the License at
 7 |  *
 8 |  *     http://www.apache.org/licenses/LICENSE-2.0
 9 |  *
10 |  * Unless required by applicable law or agreed to in writing, software
11 |  * distributed under the License is distributed on an "AS IS" BASIS,
12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 |  * See the License for the specific language governing permissions and
14 |  * limitations under the License.
15 |  */
16 | 
17 | /******************************************************************************
18 |  * Copyright 2017 The Apollo Authors. All Rights Reserved.
19 |  *
20 |  * Licensed under the Apache License, Version 2.0 (the "License");
21 |  * you may not use this file except in compliance with the License.
22 |  * You may obtain a copy of the License at
23 |  *
24 |  * http://www.apache.org/licenses/LICENSE-2.0
25 |  *
26 |  * Unless required by applicable law or agreed to in writing, software
27 |  * distributed under the License is distributed on an "AS IS" BASIS,
28 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
29 |  * See the License for the specific language governing permissions and
30 |  * limitations under the License.
31 |  *****************************************************************************/
32 | 
33 | #ifndef MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
34 | #define MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
35 | 
36 | #include <string>
37 | 
38 | // project point cloud to 2d map. clac in which grid point is.
39 | // pointcloud to pixel
40 | inline int F2I(float val, float ori, float scale) {
41 |   return static_cast<int>(std::floor((ori - val) * scale));
42 | }
43 | 
44 | inline int Pc2Pixel(float in_pc, float in_range, float out_size) {
45 |   float inv_res = 0.5 * out_size / in_range;
46 |   return static_cast<int>(std::floor((in_range - in_pc) * inv_res));
47 | }
48 | 
49 | // retutn the distance from my car to center of the grid.
50 | // Pc means point cloud = real world scale. so transform pixel scale to real
51 | // world scale
52 | inline float Pixel2Pc(int in_pixel, float in_size, float out_range) {
53 |   float res = 2.0 * out_range / in_size;
54 |   return out_range - (static_cast<float>(in_pixel) + 0.5f) * res;
55 | }
56 | 
57 | #endif  // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_
58 | 


--------------------------------------------------------------------------------
/launch/tensorrt_bcnn.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- <arg name="INPUT_CLOUD" default="/points_raw" /> -->
 4 |   <arg name="INPUT_CLOUD" default="/velodyne_points" />
 5 | 
 6 |   <arg name="MODEL" default="model_64" />
 7 |   <arg name="ENGINE_64" default="apollo_cnn.engine" />
 8 |   <arg name="ENGINE_128" default="vls128/deploy.engine" />
 9 | 
10 |   <arg name="VIZ_CONFIDENCE_IMAGE" default="false" />
11 |   <arg name="VIZ_CLASS_IMAGE" default="false" />
12 |   <arg name="PUB_COLORED_POINTS" default="false" />
13 | 
14 |   <node pkg="tensorrt_bcnn" type="tensorrt_bcnn"
15 |         name="$(anon tensorrt_bcnn)" output="screen" >
16 | 
17 |     <rosparam if="$(eval MODEL=='model_64')" subst_value="true">
18 |       points_src: $(arg INPUT_CLOUD)
19 |       trained_model: $(arg ENGINE_64)
20 |       score_threshold: 0.8
21 |       range: 60
22 |       width: 640
23 |       height: 640
24 |       use_intensity_feature: true
25 |       use_constant_feature: true
26 |       viz_confidence_image: $(arg VIZ_CONFIDENCE_IMAGE)
27 |       viz_class_image: $(arg VIZ_CLASS_IMAGE)
28 |       pub_colored_points: $(arg PUB_COLORED_POINTS)
29 |     </rosparam>
30 | 
31 |     <rosparam if="$(eval MODEL=='model_128')" subst_value="true">
32 |       points_src: $(arg INPUT_CLOUD)
33 |       trained_model: $(arg ENGINE_128)
34 |       score_threshold: 0.8
35 |       range: 90
36 |       width: 864
37 |       height: 864
38 |       use_intensity_feature: false
39 |       use_constant_feature: false
40 |       viz_confidence_image: $(arg VIZ_CONFIDENCE_IMAGE)
41 |       viz_class_image: $(arg VIZ_CLASS_IMAGE)
42 |       pub_colored_points: $(arg PUB_COLORED_POINTS)
43 |     </rosparam>
44 | 
45 |   </node>
46 | 
47 |   <group if="$(eval VIZ_CLASS_IMAGE)" ns="bev_camera">
48 |     <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
49 |           args="0 0 -1 0 0 3.141592653589793 velodyne bev 100" />
50 |     <node name="camera_info" pkg="rostopic" type="rostopic"
51 |           args="pub camera_info sensor_msgs/CameraInfo
52 |       '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: bev},
53 |       height: 640, width: 640, distortion_model: 'plumb_bob',
54 |       D: [0],
55 |       K: [5.33, 0.0, 320, 0.0, 5.33, 320, 0.0, 0.0, 1.0],
56 |       R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
57 |       P: [5.33, 0.0, 320, 0.0, 0.0, 5.33, 320, 0.0, 0.0, 0.0, 1.0, 0.0],
58 |       binning_x: 0, binning_y: 0,
59 |       roi: {x_offset: 0, y_offset: 0, height: 640, width: 640, do_rectify: false}}' -r 2"
60 |           output="screen"/>
61 |   </group>
62 | 
63 |   <!-- <node pkg="detected_objects_visualizer" type="visualize_dhetected_objects" name="cluster_detect_visualization_01" -->
64 |   <!--         output="screen" ns="/detection/lidar_detector" /> -->
65 | 
66 | </launch>
67 | 


--------------------------------------------------------------------------------
/lib/src/UpsampleLayer.cpp:
--------------------------------------------------------------------------------
 1 | #include "UpsampleLayer.h"
 2 | 
 3 | namespace nvinfer1 {
 4 | UpsampleLayerPlugin::UpsampleLayerPlugin(const float scale,
 5 |                                          const int cudaThread /*= 512*/)
 6 |     : mScale(scale), mThreadCount(cudaThread) {}
 7 | 
 8 | UpsampleLayerPlugin::~UpsampleLayerPlugin() {}
 9 | 
10 | // create the plugin at runtime from a byte stream
11 | UpsampleLayerPlugin::UpsampleLayerPlugin(const void* data, size_t length) {
12 |   using namespace Tn;
13 |   const char *d = reinterpret_cast<const char *>(data), *a = d;
14 |   read(d, mCHW);
15 |   read(d, mDataType);
16 |   read(d, mScale);
17 |   read(d, mOutputWidth);
18 |   read(d, mOutputHeight);
19 |   read(d, mThreadCount);
20 | 
21 |   // std::cout << "read:" << a << " " << mOutputWidth<< " "
22 |   // <<mOutputHeight<<std::endl;
23 |   assert(d == a + length);
24 | }
25 | 
26 | void UpsampleLayerPlugin::serialize(void* buffer) {
27 |   using namespace Tn;
28 |   char *d = static_cast<char *>(buffer), *a = d;
29 |   write(d, mCHW);
30 |   write(d, mDataType);
31 |   write(d, mScale);
32 |   write(d, mOutputWidth);
33 |   write(d, mOutputHeight);
34 |   write(d, mThreadCount);
35 | 
36 |   // std::cout << "write:" << a << " " << mOutputHeight<< " "
37 |   // <<mOutputWidth<<std::endl;
38 |   assert(d == a + getSerializationSize());
39 | }
40 | 
41 | int UpsampleLayerPlugin::initialize() {
42 |   int inputHeight = mCHW.d[1];
43 |   int inputWidth = mCHW.d[2];
44 | 
45 |   mOutputHeight = inputHeight * mScale;
46 |   mOutputWidth = inputWidth * mScale;
47 | 
48 |   return 0;
49 | }
50 | 
51 | void UpsampleLayerPlugin::configureWithFormat(
52 |     const Dims* inputDims, int nbInputs, const Dims* outputDims, int nbOutputs,
53 |     DataType type, PluginFormat format, int maxBatchSize) {
54 |   // std::cout << "type " << int(type) << "format " << (int)format <<std::endl;
55 |   assert((type == DataType::kFLOAT || type == DataType::kHALF ||
56 |           type == DataType::kINT8) &&
57 |          format == PluginFormat::kNCHW);
58 |   mDataType = type;
59 | 
60 |   // std::cout << "configureWithFormat:" <<inputDims[0].d[0]<< " "
61 |   // <<inputDims[0].d[1] << " "<<inputDims[0].d[2] <<std::endl;
62 | }
63 | 
64 | // it is called prior to any call to initialize().
65 | Dims UpsampleLayerPlugin::getOutputDimensions(int index, const Dims* inputs,
66 |                                               int nbInputDims) {
67 |   // std::cout <<"Input:" << inputs[0].d[0] << " "<<inputs[0].d[1]<< "
68 |   // "<<inputs[0].d[2]<<std::endl;
69 |   // std::cout <<"nbInputDims : "<<nbInputDims<< " input:" << inputs[0].nbDims
70 |   // << std::endl;
71 | 
72 |   mCHW = inputs[0];
73 |   mOutputHeight = inputs[0].d[1] * mScale;
74 |   mOutputWidth = inputs[0].d[2] * mScale;
75 |   // std::cout << "ouputDims:" << mCHW.d[0] << " " << mOutputHeight << " " <<
76 |   // mOutputWidth << std::endl;
77 |   return Dims3(mCHW.d[0], mOutputHeight, mOutputWidth);
78 | }
79 | }
80 | 


--------------------------------------------------------------------------------
/lib/include/TrtNet.h:
--------------------------------------------------------------------------------
 1 | #ifndef __TRT_NET_H_
 2 | #define __TRT_NET_H_
 3 | 
 4 | #include <algorithm>
 5 | #include <fstream>
 6 | #include <numeric>
 7 | #include <string>
 8 | #include <vector>
 9 | #include "NvCaffeParser.h"
10 | #include "NvInferPlugin.h"
11 | #include "PluginFactory.h"
12 | #include "Utils.h"
13 | 
14 | namespace Tn {
15 | enum class RUN_MODE { FLOAT32 = 0, FLOAT16 = 1, INT8 = 2 };
16 | 
17 | class trtNet {
18 |  public:
19 |   // Load from caffe model
20 |   trtNet(const std::string& prototxt, const std::string& caffeModel,
21 |          const std::vector<std::string>& outputNodesName,
22 |          const std::vector<std::vector<float>>& calibratorData,
23 |          RUN_MODE mode = RUN_MODE::FLOAT32);
24 | 
25 |   // Load from engine file
26 |   explicit trtNet(const std::string& engineFile);
27 | 
28 |   ~trtNet() {
29 |     // Release the stream and the buffers
30 |     cudaStreamSynchronize(mTrtCudaStream);
31 |     cudaStreamDestroy(mTrtCudaStream);
32 |     for (auto& item : mTrtCudaBuffer) cudaFree(item);
33 | 
34 |     mTrtPluginFactory.destroyPlugin();
35 | 
36 |     if (!mTrtRunTime) mTrtRunTime->destroy();
37 |     if (!mTrtContext) mTrtContext->destroy();
38 |     if (!mTrtEngine) mTrtEngine->destroy();
39 |   };
40 | 
41 |   void saveEngine(std::string fileName) {
42 |     if (mTrtEngine) {
43 |       nvinfer1::IHostMemory* data = mTrtEngine->serialize();
44 |       std::ofstream file;
45 |       file.open(fileName, std::ios::binary | std::ios::out);
46 |       if (!file.is_open()) {
47 |         std::cout << "read create engine file" << fileName << " failed"
48 |                   << std::endl;
49 |         return;
50 |       }
51 | 
52 |       file.write((const char*)data->data(), data->size());
53 |       file.close();
54 |     }
55 |   };
56 | 
57 |   void doInference(const void* inputData, void* outputData);
58 | 
59 |   inline size_t getInputSize() {
60 |     return std::accumulate(mTrtBindBufferSize.begin(),
61 |                            mTrtBindBufferSize.begin() + mTrtInputCount, 0);
62 |   };
63 | 
64 |   inline size_t getOutputSize() {
65 |     return std::accumulate(mTrtBindBufferSize.begin() + mTrtInputCount,
66 |                            mTrtBindBufferSize.end(), 0);
67 |   };
68 | 
69 |   void printTime() { mTrtProfiler.printLayerTimes(mTrtIterationTime); }
70 | 
71 |  private:
72 |   nvinfer1::ICudaEngine* loadModelAndCreateEngine(
73 |       const char* deployFile, const char* modelFile, int maxBatchSize,
74 |       nvcaffeparser1::ICaffeParser* parser,
75 |       nvcaffeparser1::IPluginFactory* pluginFactory,
76 |       nvinfer1::IInt8Calibrator* calibrator,
77 |       nvinfer1::IHostMemory*& trtModelStream,
78 |       const std::vector<std::string>& outputNodesName);
79 | 
80 |   void InitEngine();
81 | 
82 |   nvinfer1::IExecutionContext* mTrtContext;
83 |   nvinfer1::ICudaEngine* mTrtEngine;
84 |   nvinfer1::IRuntime* mTrtRunTime;
85 |   PluginFactory mTrtPluginFactory;
86 |   cudaStream_t mTrtCudaStream;
87 |   Profiler mTrtProfiler;
88 |   RUN_MODE mTrtRunMode;
89 | 
90 |   std::vector<void*> mTrtCudaBuffer;
91 |   std::vector<int64_t> mTrtBindBufferSize;
92 |   int mTrtInputCount;
93 |   int mTrtIterationTime;
94 | };
95 | }
96 | 
97 | #endif  //__TRT_NET_H_
98 | 


--------------------------------------------------------------------------------
/lib/include/Utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef __TRT_UTILS_H_
 2 | #define __TRT_UTILS_H_
 3 | 
 4 | #include <cudnn.h>
 5 | #include <algorithm>
 6 | #include <iostream>
 7 | #include <vector>
 8 | 
 9 | #ifndef CUDA_CHECK
10 | 
11 | #define CUDA_CHECK(callstr)                                                 \
12 |   {                                                                         \
13 |     cudaError_t error_code = callstr;                                       \
14 |     if (error_code != cudaSuccess) {                                        \
15 |       std::cerr << "CUDA error " << error_code << " at " << __FILE__ << ":" \
16 |                 << __LINE__;                                                \
17 |       assert(0);                                                            \
18 |     }                                                                       \
19 |   }
20 | 
21 | #endif
22 | 
23 | namespace Tn {
24 | class Profiler : public nvinfer1::IProfiler {
25 |  public:
26 |   void printLayerTimes(int itrationsTimes) {
27 |     float totalTime = 0;
28 |     for (size_t i = 0; i < mProfile.size(); i++) {
29 |       printf("%-40.40s %4.3fms\n", mProfile[i].first.c_str(),
30 |              mProfile[i].second / itrationsTimes);
31 |       totalTime += mProfile[i].second;
32 |     }
33 |     printf("Time over all layers: %4.3f\n", totalTime / itrationsTimes);
34 |   }
35 | 
36 |  private:
37 |   typedef std::pair<std::string, float> Record;
38 |   std::vector<Record> mProfile;
39 | 
40 |   virtual void reportLayerTime(const char* layerName, float ms) {
41 |     auto record =
42 |         std::find_if(mProfile.begin(), mProfile.end(),
43 |                      [&](const Record& r) { return r.first == layerName; });
44 |     if (record == mProfile.end())
45 |       mProfile.push_back(std::make_pair(layerName, ms));
46 |     else
47 |       record->second += ms;
48 |   }
49 | };
50 | 
51 | // Logger for TensorRT info/warning/errors
52 | class Logger : public nvinfer1::ILogger {
53 |  public:
54 |   Logger() : Logger(Severity::kWARNING) {}
55 | 
56 |   Logger(Severity severity) : reportableSeverity(severity) {}
57 | 
58 |   void log(Severity severity, const char* msg) override {
59 |     // suppress messages with severity enum value greater than the reportable
60 |     if (severity > reportableSeverity) return;
61 | 
62 |     switch (severity) {
63 |       case Severity::kINTERNAL_ERROR:
64 |         std::cerr << "INTERNAL_ERROR: ";
65 |         break;
66 |       case Severity::kERROR:
67 |         std::cerr << "ERROR: ";
68 |         break;
69 |       case Severity::kWARNING:
70 |         std::cerr << "WARNING: ";
71 |         break;
72 |       case Severity::kINFO:
73 |         std::cerr << "INFO: ";
74 |         break;
75 |       default:
76 |         std::cerr << "UNKNOWN: ";
77 |         break;
78 |     }
79 |     std::cerr << msg << std::endl;
80 |   }
81 | 
82 |   Severity reportableSeverity{Severity::kWARNING};
83 | };
84 | 
85 | template <typename T>
86 | void write(char*& buffer, const T& val) {
87 |   *reinterpret_cast<T*>(buffer) = val;
88 |   buffer += sizeof(T);
89 | }
90 | 
91 | template <typename T>
92 | void read(const char*& buffer, T& val) {
93 |   val = *reinterpret_cast<const T*>(buffer);
94 |   buffer += sizeof(T);
95 | }
96 | }
97 | 
98 | #endif


--------------------------------------------------------------------------------
/lib/src/UpsampleLayer.cu:
--------------------------------------------------------------------------------
 1 | #include "UpsampleLayer.h"
 2 | 
 3 | namespace nvinfer1 {
 4 | __device__ int translate_idx(int ii, int d1, int d2, int d3, int scale_factor) {
 5 |   int x, y, z, w;
 6 |   w = ii % d3;
 7 |   ii = ii / d3;
 8 |   z = ii % d2;
 9 |   ii = ii / d2;
10 |   y = ii % d1;
11 |   ii = ii / d1;
12 |   x = ii;
13 |   w = w / scale_factor;
14 |   z = z / scale_factor;
15 |   d2 /= scale_factor;
16 |   d3 /= scale_factor;
17 |   return (((x * d1 + y) * d2) + z) * d3 + w;
18 | }
19 | 
20 | template <typename Dtype>
21 | __global__ void upscale(const Dtype *input, Dtype *output, int no_elements,
22 |                         int scale_factor, int d1, int d2, int d3) {
23 |   int ii = threadIdx.x + blockDim.x * blockIdx.x;
24 |   if (ii >= no_elements) return;
25 |   int ipidx = translate_idx(ii, d1, d2, d3, scale_factor);
26 |   output[ii] = input[ipidx];
27 | }
28 | 
29 | template <typename Dtype>
30 | void UpsampleLayerPlugin::forwardGpu(const Dtype *input, Dtype *output, int N,
31 |                                      int C, int H, int W) {
32 |   int numElem = N * C * H * W;
33 |   upscale<<<(numElem + mThreadCount - 1) / mThreadCount, mThreadCount>>>(
34 |       input, output, numElem, mScale, C, H, W);
35 | }
36 | 
37 | size_t type2size(DataType dataType) {
38 |   size_t _size = 0;
39 |   switch (dataType) {
40 |     case DataType::kFLOAT:
41 |       _size = sizeof(float);
42 |       break;
43 |     case DataType::kHALF:
44 |       _size = sizeof(__half);
45 |       break;
46 |     case DataType::kINT8:
47 |       _size = sizeof(u_int8_t);
48 |       break;
49 |     default:
50 |       std::cerr << "error data type" << std::endl;
51 |   }
52 |   return _size;
53 | }
54 | 
55 | int UpsampleLayerPlugin::enqueue(int batchSize, const void *const *inputs,
56 |                                  void **outputs, void *workspace,
57 |                                  cudaStream_t stream) {
58 |   assert(batchSize == 1);
59 |   const int channels = mCHW.d[0];
60 |   const int64_t in_height = mCHW.d[1];
61 |   const int64_t in_width = mCHW.d[2];
62 |   const int64_t out_height = mOutputHeight;
63 |   const int64_t out_width = mOutputWidth;
64 |   int totalElems = batchSize * in_height * in_width * channels;
65 | 
66 |   // Handle no-op resizes efficiently.
67 |   if (out_height == in_height && out_width == in_width) {
68 |     CUDA_CHECK(cudaMemcpyAsync(outputs[0], inputs[0],
69 |                                totalElems * type2size(mDataType),
70 |                                cudaMemcpyDeviceToDevice, stream));
71 |     CUDA_CHECK(cudaStreamSynchronize(stream));
72 |     return 0;
73 |   }
74 |   // CUDA_CHECK(cudaStreamSynchronize(stream));
75 | 
76 |   switch (mDataType) {
77 |     case DataType::kFLOAT:
78 |       forwardGpu<float>((const float *)inputs[0], (float *)outputs[0],
79 |                         batchSize, mCHW.d[0], mOutputHeight, mOutputWidth);
80 |       break;
81 |     case DataType::kHALF:
82 |       forwardGpu<__half>((const __half *)inputs[0], (__half *)outputs[0],
83 |                          batchSize, mCHW.d[0], mOutputHeight, mOutputWidth);
84 |       break;
85 |     case DataType::kINT8:
86 |       forwardGpu<u_int8_t>((const u_int8_t *)inputs[0], (u_int8_t *)outputs[0],
87 |                            batchSize, mCHW.d[0], mOutputHeight, mOutputWidth);
88 |       break;
89 |     default:
90 |       std::cerr << "error data type" << std::endl;
91 |   }
92 |   return 0;
93 | };
94 | }


--------------------------------------------------------------------------------
/lib/include/PluginFactory.h:
--------------------------------------------------------------------------------
 1 | #ifndef __PLUGIN_FACTORY_H_
 2 | #define __PLUGIN_FACTORY_H_
 3 | 
 4 | #include <memory>
 5 | #include <regex>
 6 | #include <vector>
 7 | #include "NvCaffeParser.h"
 8 | #include "NvInferPlugin.h"
 9 | #include "UpsampleLayer.h"
10 | 
11 | namespace Tn {
12 | static constexpr float NEG_SLOPE = 0.1;
13 | static constexpr float UPSAMPLE_SCALE = 2.0;
14 | static constexpr int CUDA_THREAD_NUM = 512;
15 | 
16 | // Integration for serialization.
17 | using nvinfer1::plugin::INvPlugin;
18 | using nvinfer1::plugin::createPReLUPlugin;
19 | using nvinfer1::UpsampleLayerPlugin;
20 | class PluginFactory : public nvinfer1::IPluginFactory,
21 |                       public nvcaffeparser1::IPluginFactoryExt {
22 |  public:
23 |   inline bool isLeakyRelu(const char* layerName) {
24 |     return std::regex_match(layerName, std::regex(R"(layer(\d*)-act)"));
25 |   }
26 | 
27 |   inline bool isUpsample(const char* layerName) {
28 |     return std::regex_match(layerName, std::regex(R"(layer(\d*)-upsample)"));
29 |   }
30 | 
31 |   virtual nvinfer1::IPlugin* createPlugin(const char* layerName,
32 |                                           const nvinfer1::Weights* weights,
33 |                                           int nbWeights) override {
34 |     assert(isPlugin(layerName));
35 | 
36 |     if (isLeakyRelu(layerName)) {
37 |       assert(nbWeights == 0 && weights == nullptr);
38 |       mPluginLeakyRelu.emplace_back(
39 |           std::unique_ptr<INvPlugin, void (*)(INvPlugin*)>(
40 |               createPReLUPlugin(NEG_SLOPE), nvPluginDeleter));
41 |       return mPluginLeakyRelu.back().get();
42 |     } else if (isUpsample(layerName)) {
43 |       assert(nbWeights == 0 && weights == nullptr);
44 |       mPluginUpsample.emplace_back(std::unique_ptr<UpsampleLayerPlugin>(
45 |           new UpsampleLayerPlugin(UPSAMPLE_SCALE, CUDA_THREAD_NUM)));
46 |       return mPluginUpsample.back().get();
47 |     } else {
48 |       assert(0);
49 |       return nullptr;
50 |     }
51 |   }
52 | 
53 |   nvinfer1::IPlugin* createPlugin(const char* layerName, const void* serialData,
54 |                                   size_t serialLength) override {
55 |     assert(isPlugin(layerName));
56 | 
57 |     if (isLeakyRelu(layerName)) {
58 |       mPluginLeakyRelu.emplace_back(
59 |           std::unique_ptr<INvPlugin, void (*)(INvPlugin*)>(
60 |               createPReLUPlugin(serialData, serialLength), nvPluginDeleter));
61 |       return mPluginLeakyRelu.back().get();
62 |     } else if (isUpsample(layerName)) {
63 |       mPluginUpsample.emplace_back(std::unique_ptr<UpsampleLayerPlugin>(
64 |           new UpsampleLayerPlugin(serialData, serialLength)));
65 |       return mPluginUpsample.back().get();
66 |     } else {
67 |       assert(0);
68 |       return nullptr;
69 |     }
70 |   }
71 | 
72 |   bool isPlugin(const char* name) override { return isPluginExt(name); }
73 | 
74 |   bool isPluginExt(const char* name) override {
75 |     return isLeakyRelu(name) || isUpsample(name);
76 |   }
77 | 
78 |   // The application has to destroy the plugin when it knows it's safe to do so.
79 |   void destroyPlugin() {
80 |     for (auto& item : mPluginLeakyRelu) item.reset();
81 | 
82 |     for (auto& item : mPluginUpsample) item.reset();
83 |   }
84 | 
85 |   void (*nvPluginDeleter)(INvPlugin*){[](INvPlugin* ptr) {
86 |     if (ptr) ptr->destroy();
87 |   }};
88 | 
89 |   std::vector<std::unique_ptr<INvPlugin, void (*)(INvPlugin*)>>
90 |       mPluginLeakyRelu{};
91 |   std::vector<std::unique_ptr<UpsampleLayerPlugin>> mPluginUpsample{};
92 | };
93 | }
94 | 
95 | #endif
96 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(tensorrt_bcnn)
  3 | 
  4 | # set flags for CUDA availability
  5 | option(CUDA_AVAIL "CUDA available" OFF)
  6 | find_package(CUDA)
  7 | if (CUDA_FOUND)
  8 |   message("CUDA is available!")
  9 |   message("CUDA Libs: ${CUDA_LIBRARIES}")
 10 |   message("CUDA Headers: ${CUDA_INCLUDE_DIRS}")
 11 |   set(CUDA_AVAIL ON)
 12 | else()
 13 |   message("CUDA NOT FOUND")
 14 |   set(CUDA_AVAIL OFF)
 15 | endif (CUDA_FOUND)
 16 | 
 17 | # set flags for TensorRT availability
 18 | option(TRT_AVAIL "TensorRT available" OFF)
 19 | # try to find the tensorRT modules
 20 | find_library(NVINFER NAMES nvinfer)
 21 | find_library(NVPARSERS NAMES nvparsers)
 22 | find_library(NVCAFFE_PARSER NAMES nvcaffe_parser)
 23 | find_library(NVINFER_PLUGIN NAMES nvinfer_plugin)
 24 | if(NVINFER AND NVPARSERS AND NVCAFFE_PARSER AND NVINFER_PLUGIN)
 25 |    message("TensorRT is available!")
 26 |    message("NVINFER: ${NVINFER}")
 27 |    message("NVPARSERS: ${NVPARSERS}")
 28 |    message("NVCAFFE_PARSER: ${NVCAFFE_PARSER}")
 29 |    set(TRT_AVAIL ON)
 30 | else()
 31 |   message("TensorRT is NOT Available")
 32 |   set(TRT_AVAIL OFF)
 33 | endif()
 34 | 
 35 | # set flags for CUDNN availability
 36 | option(CUDNN_AVAIL "CUDNN available" OFF)
 37 | # try to find the CUDNN module
 38 | find_library(CUDNN_LIBRARY
 39 | NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name}
 40 | PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX}
 41 | PATH_SUFFIXES lib lib64 bin
 42 | DOC "CUDNN library." )
 43 | if(CUDNN_LIBRARY)
 44 |    message("CUDNN is available!")
 45 |    message("CUDNN_LIBRARY: ${CUDNN_LIBRARY}")
 46 |    set(CUDNN_AVAIL ON)
 47 | else()
 48 |   message("CUDNN is NOT Available")
 49 |   set(CUDNN_AVAIL OFF)
 50 | endif()
 51 | 
 52 | if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
 53 | 
 54 |   find_package(catkin REQUIRED COMPONENTS
 55 |    roscpp
 56 |    roslib
 57 |    pcl_ros
 58 |    autoware_perception_msgs
 59 |    cv_bridge
 60 |    )
 61 |   set(CMAKE_CXX_STANDARD 11)
 62 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
 63 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_MWAITXINTRIN_H_INCLUDED")
 64 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_FORCE_INLINES")
 65 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__STRICT_ANSI__")
 66 | 
 67 |   catkin_package(
 68 |    CATKIN_DEPENDS
 69 |    roscpp
 70 |    roslib
 71 |    pcl_ros
 72 |    autoware_perception_msgs
 73 |    cv_bridge
 74 |    )
 75 | 
 76 |   include_directories(
 77 |    include
 78 |    lib/include
 79 |    ${catkin_INCLUDE_DIRS}
 80 |   )
 81 |   set(SOURCE_FILES
 82 |    src/tensorrt_bcnn_main.cpp
 83 |    src/tensorrt_bcnn_ros.cpp
 84 |    src/feature_generator.cpp
 85 |    src/cluster2d.cpp
 86 |    )
 87 | 
 88 |   add_executable(tensorrt_bcnn
 89 |          ${SOURCE_FILES})
 90 | 
 91 |   add_dependencies(tensorrt_bcnn
 92 |          ${catkin_EXPORTED_TARGETS}
 93 |          )
 94 | 
 95 | 
 96 |   cuda_add_library(gpu_tensorrt_bcnn_lib
 97 |          lib/src/UpsampleLayer.cu
 98 |          )
 99 | 
100 |   target_link_libraries(gpu_tensorrt_bcnn_lib
101 |    ${CUDA_LIBRARIES}
102 |   )
103 | 
104 |   add_library(tensorrt_bcnn_lib
105 |          lib/src/TrtNet.cpp
106 |          lib/src/UpsampleLayer.cpp
107 |          )
108 | 
109 |   target_link_libraries(tensorrt_bcnn_lib
110 |          ${NVINFER}
111 |          ${NVCAFFE_PARSER}
112 |          ${NVINFER_PLUGIN}
113 |          ${CUDA_LIBRARIES}
114 |          ${CUDA_CUBLAS_LIBRARIES}
115 |          ${CUDA_curand_LIBRARY}
116 |          ${CUDNN_LIBRARY}
117 |          gpu_tensorrt_bcnn_lib
118 |          )
119 | 
120 |   target_link_libraries(tensorrt_bcnn
121 |          ${catkin_LIBRARIES}
122 |          tensorrt_bcnn_lib
123 |          )
124 | 
125 | 
126 |   install(TARGETS
127 |          gpu_tensorrt_bcnn_lib
128 |          tensorrt_bcnn_lib
129 |          tensorrt_bcnn
130 |          ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
131 |          LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
132 |          RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
133 |   )
134 | 
135 |   install(DIRECTORY launch/
136 |          DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
137 |          PATTERN ".svn" EXCLUDE)
138 | 
139 |   install(DIRECTORY include/
140 |          DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}/${PROJECT_NAME}/
141 |          PATTERN ".svn" EXCLUDE
142 |          )
143 | 
144 | else()
145 |   find_package(catkin REQUIRED)
146 |   catkin_package()
147 | endif()
148 | 


--------------------------------------------------------------------------------
/lib/src/TrtNet.cpp:
--------------------------------------------------------------------------------
  1 | #include "TrtNet.h"
  2 | #include <cublas_v2.h>
  3 | #include <cudnn.h>
  4 | #include <string.h>
  5 | #include <time.h>
  6 | #include <cassert>
  7 | #include <chrono>
  8 | #include <iostream>
  9 | #include <sstream>
 10 | #include <unordered_map>
 11 | 
 12 | using namespace nvinfer1;
 13 | using namespace nvcaffeparser1;
 14 | using namespace plugin;
 15 | 
 16 | static Tn::Logger gLogger;
 17 | 
 18 | inline void* safeCudaMalloc(size_t memSize) {
 19 |   void* deviceMem;
 20 |   CUDA_CHECK(cudaMalloc(&deviceMem, memSize));
 21 |   if (deviceMem == nullptr) {
 22 |     std::cerr << "Out of memory" << std::endl;
 23 |     exit(1);
 24 |   }
 25 |   return deviceMem;
 26 | }
 27 | 
 28 | inline int64_t volume(const nvinfer1::Dims& d) {
 29 |   return std::accumulate(d.d, d.d + d.nbDims, 1, std::multiplies<int64_t>());
 30 | }
 31 | 
 32 | inline unsigned int getElementSize(nvinfer1::DataType t) {
 33 |   switch (t) {
 34 |     case nvinfer1::DataType::kINT32:
 35 |       return 4;
 36 |     case nvinfer1::DataType::kFLOAT:
 37 |       return 4;
 38 |     case nvinfer1::DataType::kHALF:
 39 |       return 2;
 40 |     case nvinfer1::DataType::kINT8:
 41 |       return 1;
 42 |   }
 43 |   throw std::runtime_error("Invalid DataType.");
 44 |   return 0;
 45 | }
 46 | 
 47 | namespace Tn {
 48 | trtNet::trtNet(const std::string& engineFile)
 49 |     : mTrtContext(nullptr),
 50 |       mTrtEngine(nullptr),
 51 |       mTrtRunTime(nullptr),
 52 |       mTrtRunMode(RUN_MODE::FLOAT32),
 53 |       mTrtInputCount(0),
 54 |       mTrtIterationTime(0) {
 55 |   using namespace std;
 56 |   fstream file;
 57 | 
 58 |   file.open(engineFile, ios::binary | ios::in);
 59 |   if (!file.is_open()) {
 60 |     cout << "read engine file" << engineFile << " failed" << endl;
 61 |     return;
 62 |   }
 63 |   file.seekg(0, ios::end);
 64 |   int length = file.tellg();
 65 |   file.seekg(0, ios::beg);
 66 |   std::unique_ptr<char[]> data(new char[length]);
 67 |   file.read(data.get(), length);
 68 | 
 69 |   file.close();
 70 | 
 71 |   // std::cout << "*** deserializing" << std::endl;
 72 |   mTrtRunTime = createInferRuntime(gLogger);
 73 |   assert(mTrtRunTime != nullptr);
 74 |   mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length,
 75 |                                                   &mTrtPluginFactory);
 76 |   assert(mTrtEngine != nullptr);
 77 | 
 78 |   InitEngine();
 79 |   // std::cerr << "finised deserializing " << std::endl;
 80 | }
 81 | 
 82 | void trtNet::InitEngine() {
 83 |   const int maxBatchSize = 1;
 84 |   mTrtContext = mTrtEngine->createExecutionContext();
 85 |   assert(mTrtContext != nullptr);
 86 |   mTrtContext->setProfiler(&mTrtProfiler);
 87 | 
 88 |   // Input and output buffer pointers that we pass to the engine - the engine
 89 |   // requires exactly IEngine::getNbBindings()
 90 |   int nbBindings = mTrtEngine->getNbBindings();
 91 | 
 92 |   mTrtCudaBuffer.resize(nbBindings);
 93 |   mTrtBindBufferSize.resize(nbBindings);
 94 |   for (int i = 0; i < nbBindings; ++i) {
 95 |     Dims dims = mTrtEngine->getBindingDimensions(i);
 96 |     DataType dtype = mTrtEngine->getBindingDataType(i);
 97 |     int64_t totalSize = volume(dims) * maxBatchSize * getElementSize(dtype);
 98 |     mTrtBindBufferSize[i] = totalSize;
 99 |     mTrtCudaBuffer[i] = safeCudaMalloc(totalSize);
100 |     if (mTrtEngine->bindingIsInput(i)) mTrtInputCount++;
101 |   }
102 | 
103 |   CUDA_CHECK(cudaStreamCreate(&mTrtCudaStream));
104 | }
105 | 
106 | void trtNet::doInference(const void* inputData, void* outputData)
107 | 
108 | {
109 |   static const int batchSize = 1;
110 |   assert(mTrtInputCount == 1);
111 | 
112 |   // DMA the input to the GPU,  execute the batch asynchronously, and DMA it
113 |   // back:
114 |   int inputIndex = 0;
115 |   CUDA_CHECK(cudaMemcpyAsync(mTrtCudaBuffer[inputIndex], inputData,
116 |                              mTrtBindBufferSize[inputIndex],
117 |                              cudaMemcpyHostToDevice, mTrtCudaStream));
118 |   auto t_start = std::chrono::high_resolution_clock::now();
119 | 
120 |   mTrtContext->execute(batchSize, &mTrtCudaBuffer[inputIndex]);
121 | 
122 |   auto t_end = std::chrono::high_resolution_clock::now();
123 |   float total =
124 |       std::chrono::duration<float, std::milli>(t_end - t_start).count();
125 | 
126 |   for (size_t bindingIdx = mTrtInputCount;
127 |        bindingIdx < mTrtBindBufferSize.size(); ++bindingIdx) {
128 |     auto size = mTrtBindBufferSize[bindingIdx];
129 |     CUDA_CHECK(cudaMemcpyAsync(outputData, mTrtCudaBuffer[bindingIdx], size,
130 |                                cudaMemcpyDeviceToHost, mTrtCudaStream));
131 |   }
132 | 
133 |   mTrtIterationTime++;
134 | }
135 | }
136 | 


--------------------------------------------------------------------------------
/src/feature_generator.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
  3 |  *
  4 |  * Licensed under the Apache License, Version 2.0 (the "License");
  5 |  * you may not use this file except in compliance with the License.
  6 |  * You may obtain a copy of the License at
  7 |  *
  8 |  *     http://www.apache.org/licenses/LICENSE-2.0
  9 |  *
 10 |  * Unless required by applicable law or agreed to in writing, software
 11 |  * distributed under the License is distributed on an "AS IS" BASIS,
 12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13 |  * See the License for the specific language governing permissions and
 14 |  * limitations under the License.
 15 |  */
 16 | 
 17 | #include "feature_generator.h"
 18 | 
 19 | bool FeatureGenerator::init(float *in_feature_ptr, int range, int width,
 20 |                             int height, const bool use_constant_feature,
 21 |                             const bool use_intensity_feature) {
 22 |   range_ = range;
 23 |   width_ = width;
 24 |   height_ = height;
 25 | 
 26 |   min_height_ = -5.0;
 27 |   max_height_ = 5.0;
 28 | 
 29 |   log_table_.resize(256);
 30 |   for (size_t i = 0; i < log_table_.size(); ++i) {
 31 |     log_table_[i] = std::log1p(static_cast<float>(i));
 32 |   }
 33 | 
 34 |   int siz = height_ * width_;
 35 |   float *direction_data, *distance_data;
 36 |   if (use_constant_feature && use_intensity_feature) {
 37 |     direction_data = in_feature_ptr + siz * 3;
 38 |     distance_data = in_feature_ptr + siz * 6;
 39 |   } else if (use_constant_feature) {
 40 |     direction_data = in_feature_ptr + siz * 3;
 41 |     distance_data = in_feature_ptr + siz * 4;
 42 |   }
 43 | 
 44 |   if (use_constant_feature) {
 45 |     for (int row = 0; row < height_; ++row) {
 46 |       for (int col = 0; col < width_; ++col) {
 47 |         int idx = row * width_ + col;
 48 |         // * row <-> x, column <-> y
 49 |         // retutn the distance from my car to center of the grid.
 50 |         // Pc means point cloud = real world scale. so transform pixel scale to
 51 |         // real world scale
 52 |         float center_x = Pixel2Pc(row, height_, range_);
 53 |         float center_y = Pixel2Pc(col, width_, range_);
 54 |         constexpr double K_CV_PI = 3.1415926535897932384626433832795;
 55 |         // normaliztion. -0.5~0.5
 56 |         direction_data[idx] = static_cast<float>(
 57 |             std::atan2(center_y, center_x) / (2.0 * K_CV_PI));
 58 |         distance_data[idx] =
 59 |             static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
 60 |       }
 61 |     }
 62 |   }
 63 | 
 64 |   return true;
 65 | }
 66 | 
 67 | float FeatureGenerator::logCount(int count) {
 68 |   if (count < static_cast<int>(log_table_.size())) {
 69 |     return log_table_[count];
 70 |   }
 71 |   return std::log(static_cast<float>(1 + count));
 72 | }
 73 | 
 74 | void FeatureGenerator::generate(
 75 |     const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr, float *max_height_data,
 76 |     const bool use_constant_feature, const bool use_intensity_feature) {
 77 |   const auto &points = pc_ptr->points;
 78 |   int siz = height_ * width_;
 79 |   float *mean_height_data, *count_data, *top_intensity_data,
 80 |       *mean_intensity_data, *nonempty_data;
 81 | 
 82 |   mean_height_data = max_height_data + siz;
 83 |   count_data = max_height_data + siz * 2;
 84 | 
 85 |   if (use_constant_feature && use_intensity_feature) {
 86 |     top_intensity_data = max_height_data + siz * 4;
 87 |     mean_intensity_data = max_height_data + siz * 5;
 88 |     nonempty_data = max_height_data + siz * 7;
 89 |   } else if (use_constant_feature) {
 90 |     nonempty_data = max_height_data + siz * 5;
 91 |   } else if (use_intensity_feature) {
 92 |     top_intensity_data = max_height_data + siz * 3;
 93 |     mean_intensity_data = max_height_data + siz * 4;
 94 |     nonempty_data = max_height_data + siz * 5;
 95 |   } else {
 96 |     nonempty_data = max_height_data + siz * 3;
 97 |   }
 98 | 
 99 |   map_idx_.resize(points.size());
100 |   float inv_res_x =
101 |       0.5 * static_cast<float>(width_) / static_cast<float>(range_);
102 |   float inv_res_y =
103 |       0.5 * static_cast<float>(height_) / static_cast<float>(range_);
104 | 
105 |   for (size_t i = 0; i < points.size(); ++i) {
106 |     if (points[i].z <= min_height_ || points[i].z >= max_height_) {
107 |       map_idx_[i] = -1;
108 |       continue;
109 |     }
110 |     // project point cloud to 2d map. clac in which grid point is.
111 |     // * the coordinates of x and y are exchanged here
112 |     // (row <-> x, column <-> y)
113 |     int pos_x = F2I(points[i].y, range_, inv_res_x);
114 |     int pos_y = F2I(points[i].x, range_, inv_res_y);
115 |     if (pos_x >= width_ || pos_x < 0 || pos_y >= height_ || pos_y < 0) {
116 |       map_idx_[i] = -1;
117 |       continue;
118 |     }
119 |     map_idx_[i] = pos_y * width_ + pos_x;
120 |     int idx = map_idx_[i];
121 |     float pz = points[i].z;
122 |     float pi = points[i].intensity / 255.0;
123 |     if (max_height_data[idx] < pz) {
124 |       max_height_data[idx] = pz;
125 |       top_intensity_data[idx] = pi;  // not I_max but I of z_max ?
126 |     }
127 |     mean_height_data[idx] += static_cast<float>(pz);
128 |     mean_intensity_data[idx] += static_cast<float>(pi);
129 |     count_data[idx] += static_cast<float>(1);
130 |   }
131 |   for (int i = 0; i < siz; ++i) {
132 |     constexpr double EPS = 1e-6;
133 |     if (count_data[i] < EPS) {
134 |       max_height_data[i] = static_cast<float>(0);
135 |     } else {
136 |       mean_height_data[i] /= count_data[i];
137 |       mean_intensity_data[i] /= count_data[i];
138 |       nonempty_data[i] = static_cast<float>(1);
139 |     }
140 |     count_data[i] = logCount(static_cast<int>(count_data[i]));
141 |   }
142 | }
143 | 


--------------------------------------------------------------------------------
/include/cluster2d.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
  3 |  *
  4 |  * Licensed under the Apache License, Version 2.0 (the "License");
  5 |  * you may not use this file except in compliance with the License.
  6 |  * You may obtain a copy of the License at
  7 |  *
  8 |  *     http://www.apache.org/licenses/LICENSE-2.0
  9 |  *
 10 |  * Unless required by applicable law or agreed to in writing, software
 11 |  * distributed under the License is distributed on an "AS IS" BASIS,
 12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13 |  * See the License for the specific language governing permissions and
 14 |  * limitations under the License.
 15 |  */
 16 | 
 17 | /*
 18 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
 19 |  *
 20 |  * Licensed under the Apache License, Version 2.0 (the "License");
 21 |  * you may not use this file except in compliance with the License.
 22 |  * You may obtain a copy of the License at
 23 |  *
 24 |  *     http://www.apache.org/licenses/LICENSE-2.0
 25 |  *
 26 |  * Unless required by applicable law or agreed to in writing, software
 27 |  * distributed under the License is distributed on an "AS IS" BASIS,
 28 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 29 |  * See the License for the specific language governing permissions and
 30 |  * limitations under the License.
 31 |  */
 32 | 
 33 | #ifndef CLUSTER2D_H
 34 | #define CLUSTER2D_H
 35 | 
 36 | #include <pcl/point_types.h>
 37 | #include <pcl_ros/point_cloud.h>
 38 | #include <vector>
 39 | 
 40 | #include "disjoint_set.h"
 41 | #include "util.h"
 42 | 
 43 | // #include <autoware_perception_msgs/DynamicObjectWithFeature.h>
 44 | // #include <autoware_perception_msgs/DynamicObjectWithFeatureArray.h>
 45 | #include <autoware_perception_msgs/DynamicObjectWithFeature.h>
 46 | #include <autoware_perception_msgs/DynamicObjectWithFeatureArray.h>
 47 | 
 48 | #include <std_msgs/Header.h>
 49 | 
 50 | #include <opencv2/highgui/highgui.hpp>
 51 | #include "opencv2/core/core.hpp"
 52 | #include "opencv2/imgproc/imgproc.hpp"
 53 | 
 54 | enum ObjectType {
 55 |   UNKNOWN = 0,
 56 |   UNKNOWN_MOVABLE = 1,
 57 |   UNKNOWN_UNMOVABLE = 2,
 58 |   PEDESTRIAN = 3,
 59 |   BICYCLE = 4,
 60 |   VEHICLE = 5,
 61 |   MAX_OBJECT_TYPE = 6,
 62 | };
 63 | 
 64 | enum MetaType {
 65 |   META_UNKNOWN,
 66 |   META_SMALLMOT,
 67 |   META_BIGMOT,
 68 |   META_NONMOT,
 69 |   META_PEDESTRIAN,
 70 |   MAX_META_TYPE
 71 | };
 72 | 
 73 | struct Obstacle {
 74 |   std::vector<int> grids;
 75 |   pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr;
 76 |   float score;
 77 |   float height;
 78 |   MetaType meta_type;
 79 |   std::vector<float> meta_type_probs;
 80 | 
 81 |   Obstacle() : score(0.0), height(-5.0), meta_type(META_UNKNOWN) {
 82 |     cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
 83 |     meta_type_probs.assign(MAX_META_TYPE, 0.0);
 84 |   }
 85 | 
 86 |   std::string GetTypeString() const {
 87 |     switch (meta_type) {
 88 |       case META_UNKNOWN:
 89 |         return "unknown";
 90 |       case META_SMALLMOT:
 91 |         return "car";
 92 |       case META_BIGMOT:
 93 |         return "truck";
 94 |       case META_NONMOT:
 95 |         return "bike";
 96 |       case META_PEDESTRIAN:
 97 |         return "pedestrian";
 98 |       default:
 99 |         return "unknown";
100 |     }
101 |   }
102 | };
103 | 
104 | class Cluster2D {
105 |  public:
106 |   Cluster2D() = default;
107 | 
108 |   ~Cluster2D() = default;
109 | 
110 |   bool init(int rows, int cols, float range);
111 | 
112 |   void cluster(const float *output,
113 |                const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
114 |                const pcl::PointIndices &valid_indices, float objectness_thresh,
115 |                bool use_all_grids_for_clustering);
116 | 
117 |   void filter(const float *output);
118 |   void classify(const float *output);
119 | 
120 |   void getObjects(const float confidence_thresh, const float height_thresh,
121 |                   const int min_pts_num,
122 |                   autoware_perception_msgs::DynamicObjectWithFeatureArray &objects,
123 |                   const std_msgs::Header &in_header);
124 | 
125 |   autoware_perception_msgs::DynamicObjectWithFeature obstacleToObject(
126 |       const Obstacle &in_obstacle, const std_msgs::Header &in_header);
127 | 
128 |  private:
129 |   int rows_;
130 |   int cols_;
131 |   int siz_;
132 |   float range_;
133 |   float scale_;
134 |   float inv_res_x_;
135 |   float inv_res_y_;
136 |   std::vector<int> point2grid_;
137 |   std::vector<Obstacle> obstacles_;
138 |   std::vector<int> id_img_;
139 | 
140 |   pcl::PointCloud<pcl::PointXYZI>::Ptr pc_ptr_;
141 |   const std::vector<int> *valid_indices_in_pc_ = nullptr;
142 | 
143 |   struct Node {
144 |     Node *center_node;
145 |     Node *parent;
146 |     char node_rank;
147 |     char traversed;
148 |     bool is_center;
149 |     bool is_object;
150 |     int point_num;
151 |     int obstacle_id;
152 | 
153 |     Node() {
154 |       center_node = nullptr;
155 |       parent = nullptr;
156 |       node_rank = 0;
157 |       traversed = 0;
158 |       is_center = false;
159 |       is_object = false;
160 |       point_num = 0;
161 |       obstacle_id = -1;
162 |     }
163 |   };
164 | 
165 |   inline bool IsValidRowCol(int row, int col) const {
166 |     return IsValidRow(row) && IsValidCol(col);
167 |   }
168 | 
169 |   inline bool IsValidRow(int row) const { return row >= 0 && row < rows_; }
170 | 
171 |   inline bool IsValidCol(int col) const { return col >= 0 && col < cols_; }
172 | 
173 |   inline int RowCol2Grid(int row, int col) const { return row * cols_ + col; }
174 | 
175 |   void traverse(Node *x);
176 | 
177 |   ObjectType getObjectType(const MetaType meta_type_id);
178 | };
179 | 
180 | #endif  // CLUSTER_2D_H
181 | 


--------------------------------------------------------------------------------
/src/tensorrt_bcnn_ros.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/package.h>
  2 | #include <boost/filesystem.hpp>
  3 | 
  4 | #include <algorithm>
  5 | #include <string>
  6 | #include "tensorrt_bcnn_ros.h"
  7 | 
  8 | 
  9 | TensorrtBcnnROS::TensorrtBcnnROS(/* args */) : pnh_("~") {}
 10 | bool TensorrtBcnnROS::init() {
 11 |   ros::NodeHandle private_node_handle("~");  // to receive args
 12 |   private_node_handle.param<std::string>("points_src", topic_src_,
 13 |                                          "/points_raw");
 14 |   private_node_handle.param<std::string>("trained_model", trained_model_name_,
 15 |                                          "apollo_cnn.engine");
 16 |   private_node_handle.param<float>("score_threshold", score_threshold_, 0.8);
 17 |   private_node_handle.param<int>("range", range_, 60);
 18 |   private_node_handle.param<int>("width", cols_, 640);
 19 |   private_node_handle.param<int>("height", rows_, 640);
 20 |   private_node_handle.param<bool>("use_intensity_feature",
 21 |                                   use_intensity_feature_, true);
 22 |   private_node_handle.param<bool>("use_constant_feature",
 23 |                                   use_constant_feature_, true);
 24 |   private_node_handle.param<bool>("viz_confidence_image",
 25 |                                   viz_confidence_image_, true);
 26 |   private_node_handle.param<bool>("viz_class_image",
 27 |                                   viz_class_image_, false);
 28 |   private_node_handle.param<bool>("pub_colored_points",
 29 |                                   pub_colored_points_, false);
 30 | 
 31 |   siz_ = rows_ * cols_;
 32 |   if (use_intensity_feature_) {
 33 |     channels_ += 2;
 34 |   }
 35 |   if (use_constant_feature_) {
 36 |     channels_ += 2;
 37 |   }
 38 | 
 39 |   in_feature.resize(siz_ * channels_);
 40 | 
 41 |   std::string package_path = ros::package::getPath("tensorrt_bcnn");
 42 |   std::string engine_path = package_path + "/data/" + trained_model_name_;
 43 | 
 44 |   std::ifstream fs(engine_path);
 45 |   if (fs.is_open()) {
 46 |     ROS_INFO("load %s", engine_path.c_str());
 47 |     net_ptr_.reset(new Tn::trtNet(engine_path));
 48 |   } else {
 49 |     ROS_INFO("Could not find %s.", engine_path.c_str());
 50 |   }
 51 | 
 52 |   cluster2d_.reset(new Cluster2D());
 53 |   if (!cluster2d_->init(rows_, cols_, range_)) {
 54 |     ROS_ERROR("[%s] Fail to Initialize cluster2d for CNNSegmentation",
 55 |               __APP_NAME__);
 56 |     return false;
 57 |   }
 58 | 
 59 |   feature_generator_.reset(new FeatureGenerator());
 60 |   if (!feature_generator_->init(&in_feature[0], range_, cols_, rows_,
 61 |                                 use_constant_feature_,
 62 |                                 use_intensity_feature_)) {
 63 |     ROS_ERROR("[%s] Fail to Initialize feature generator for CNNSegmentation",
 64 |               __APP_NAME__);
 65 |     return false;
 66 |   }
 67 | 
 68 |   ROS_INFO("[%s] points_src: %s", __APP_NAME__, topic_src_.c_str());
 69 | }
 70 | 
 71 | TensorrtBcnnROS::~TensorrtBcnnROS() {}
 72 | 
 73 | void TensorrtBcnnROS::createROSPubSub() {
 74 |   sub_points_ =
 75 |       nh_.subscribe(topic_src_, 1, &TensorrtBcnnROS::pointsCallback, this);
 76 |   confidence_image_pub_ =
 77 |       nh_.advertise<sensor_msgs::Image>("confidence_image", 1);
 78 |   confidence_map_pub_ =
 79 |       nh_.advertise<nav_msgs::OccupancyGrid>("confidence_map", 1);
 80 |   class_image_pub_ = nh_.advertise<sensor_msgs::Image>("class_image", 1);
 81 |   points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(
 82 |       "/detection/lidar_detector/points_cluster", 1);
 83 |   objects_pub_ =
 84 |       nh_.advertise<autoware_perception_msgs::DynamicObjectWithFeatureArray>(
 85 |           "/detection/lidar_detector/objects", 1);
 86 |   d_objects_pub_ =
 87 |       nh_.advertise<autoware_perception_msgs::DynamicObjectWithFeatureArray>(
 88 |           "labeled_clusters", 1);
 89 | }
 90 | 
 91 | void TensorrtBcnnROS::reset_in_feature() {
 92 |   if (use_constant_feature_ && use_intensity_feature_) {
 93 |     for (int i = 0; i < siz_ * 8; ++i) {
 94 |       if (i < siz_) {
 95 |         // max_height_data_
 96 |         in_feature[i] = -5;
 97 |       } else if (siz_ <= i && i < siz_ * 3) {
 98 |         // mean_height_data_, count_data_
 99 |         in_feature[i] = 0;
100 |       } else if (siz_ * 4 <= i && i < siz_ * 6) {
101 |         // top_intensity_data_, mean_intensity_data_
102 |         in_feature[i] = 0;
103 |       } else if (siz_ * 7 <= i && i < siz_ * 8) {
104 |         // nonempty_data_
105 |         in_feature[i] = 0;
106 |       }
107 |     }
108 |   }
109 | 
110 |   else if (use_constant_feature_) {
111 |     for (int i = 0; i < siz_ * 6; ++i) {
112 |       if (i < siz_) {
113 |         // max_height_data_
114 |         in_feature[i] = -5;
115 |       } else if (siz_ <= i && i < siz_ * 3) {
116 |         // mean_height_data_, count_data_
117 |         in_feature[i] = 0;
118 |       } else if (siz_ * 4 <= i && i < siz_ * 6) {
119 |         // nonempty_data_
120 |         in_feature[i] = 0;
121 |       }
122 |     }
123 |   }
124 | 
125 |   else {
126 |     for (int i = 0; i < siz_ * channels_; ++i) {
127 |       if (i < siz_) {
128 |         in_feature[i] = -5;
129 |       } else {
130 |         in_feature[i] = 0;
131 |       }
132 |     }
133 |   }
134 | }
135 | 
136 | cv::Mat TensorrtBcnnROS::get_confidence_image(const float *output) {
137 |   cv::Mat confidence_image(rows_, cols_, CV_8UC1);
138 |   for (int row = 0; row < rows_; ++row) {
139 |     unsigned char *src = confidence_image.ptr<unsigned char>(row);
140 |     for (int col = 0; col < cols_; ++col) {
141 |       int grid = row + col * rows_;
142 |       if (output[grid + siz_ * 3] > score_threshold_) {
143 |         src[cols_ - col - 1] = 255;
144 |       } else {
145 |         src[cols_ - col - 1] = 0;
146 |       }
147 |     }
148 |   }
149 |   return confidence_image;
150 | }
151 | 
152 | cv::Mat TensorrtBcnnROS::get_class_image(const float *output) {
153 |   cv::Mat class_image(rows_, cols_, CV_8UC3);
154 | 
155 |   for (int row = 0; row < rows_; ++row) {
156 |     cv::Vec3b *src = class_image.ptr<cv::Vec3b>(row);
157 |     for (int col = 0; col < cols_; ++col) {
158 |       int grid = row + col * cols_;
159 |       std::vector<float> class_vec{
160 |           output[grid + siz_ * 4], output[grid + siz_ * 5],
161 |           output[grid + siz_ * 6], output[grid + siz_ * 7],
162 |           output[grid + siz_ * 8], output[grid + siz_ * 9]};
163 |       std::vector<float>::iterator maxIt =
164 |           std::max_element(class_vec.begin(), class_vec.end());
165 |       size_t pred_class = std::distance(class_vec.begin(), maxIt);
166 |       if (pred_class == 1) {
167 |         src[cols_ - col - 1] = cv::Vec3b(255, 0, 0);
168 |       } else if (pred_class == 2) {
169 |         src[cols_ - col - 1] = cv::Vec3b(255, 160, 0);
170 |       } else if (pred_class == 3) {
171 |         src[cols_ - col - 1] = cv::Vec3b(0, 255, 0);
172 |       } else if (pred_class == 4) {
173 |         src[cols_ - col - 1] = cv::Vec3b(0, 0, 255);
174 |       } else {
175 |         src[cols_ - col - 1] = cv::Vec3b(0, 0, 0);
176 |       }
177 |     }
178 |   }
179 |   return class_image;
180 | }
181 | 
182 | nav_msgs::OccupancyGrid TensorrtBcnnROS::get_confidence_map(
183 |     cv::Mat confidence_image) {
184 |   nav_msgs::OccupancyGrid confidence_map;
185 |   confidence_map.header = message_header_;
186 |   confidence_map.info.width = cols_;
187 |   confidence_map.info.height = rows_;
188 |   confidence_map.info.origin.orientation.w = 1;
189 |   float resolution = range_ * 2. / cols_;
190 |   confidence_map.info.resolution = resolution;
191 |   confidence_map.info.origin.position.x = -((cols_ + 1) * resolution * 0.5f);
192 |   confidence_map.info.origin.position.y = -((rows_ + 1) * resolution * 0.5f);
193 |   int data;
194 |   for (int i = rows_ - 1; i >= 0; i--) {
195 |     for (int j = 0; j < cols_; j++) {
196 |       data = confidence_image.data[i * cols_ + j];
197 |       if (data == 0) {
198 |           confidence_map.data.push_back(100);
199 |       } else {
200 |           confidence_map.data.push_back(0);
201 |       }
202 |     }
203 |   }
204 |   return confidence_map;
205 | }
206 | 
207 | void TensorrtBcnnROS::pubColoredPoints(
208 |     const autoware_perception_msgs::DynamicObjectWithFeatureArray
209 |         &objects_array) {
210 |   pcl::PointCloud<pcl::PointXYZRGB> colored_cloud;
211 |   for (size_t object_i = 0; object_i < objects_array.feature_objects.size();
212 |        object_i++) {
213 |     pcl::PointCloud<pcl::PointXYZI> object_cloud;
214 |     pcl::fromROSMsg(objects_array.feature_objects.at(object_i).feature.cluster,
215 |                     object_cloud);
216 |     int red = (object_i) % 256;
217 |     int green = (object_i * 7) % 256;
218 |     int blue = (object_i * 13) % 256;
219 | 
220 |     for (size_t i = 0; i < object_cloud.size(); i++) {
221 |       pcl::PointXYZRGB colored_point;
222 |       colored_point.x = object_cloud[i].x;
223 |       colored_point.y = object_cloud[i].y;
224 |       colored_point.z = object_cloud[i].z;
225 |       colored_point.r = red;
226 |       colored_point.g = green;
227 |       colored_point.b = blue;
228 |       colored_cloud.push_back(colored_point);
229 |     }
230 |   }
231 |   sensor_msgs::PointCloud2 output_colored_cloud;
232 |   pcl::toROSMsg(colored_cloud, output_colored_cloud);
233 |   output_colored_cloud.header = message_header_;
234 |   points_pub_.publish(output_colored_cloud);
235 | }
236 | 
237 | void TensorrtBcnnROS::pointsCallback(const sensor_msgs::PointCloud2 &msg) {
238 |   pcl::PointCloud<pcl::PointXYZI>::Ptr in_pc_ptr(
239 |       new pcl::PointCloud<pcl::PointXYZI>);
240 |   pcl::fromROSMsg(msg, *in_pc_ptr);
241 |   pcl::PointIndices valid_idx;
242 |   auto &indices = valid_idx.indices;
243 |   indices.resize(in_pc_ptr->size());
244 |   std::iota(indices.begin(), indices.end(), 0);
245 |   message_header_ = msg.header;
246 |   this->reset_in_feature();
247 |   feature_generator_->generate(in_pc_ptr, &in_feature[0], use_constant_feature_,
248 |                                use_intensity_feature_);
249 | 
250 |   int outputCount = net_ptr_->getOutputSize() / sizeof(float);
251 |   std::unique_ptr<float[]> output_data(new float[outputCount]);
252 | 
253 |   net_ptr_->doInference(in_feature.data(), output_data.get());
254 |   float *output = output_data.get();
255 | 
256 |   float objectness_thresh = 0.5;
257 |   bool use_all_grids_for_clustering = true;
258 | 
259 |   cluster2d_->cluster(output, in_pc_ptr, valid_idx, objectness_thresh,
260 |                       use_all_grids_for_clustering);
261 |   cluster2d_->filter(output);
262 |   cluster2d_->classify(output);
263 | 
264 |   float confidence_thresh = score_threshold_;
265 |   float height_thresh = 0.5;
266 |   int min_pts_num = 3;
267 | 
268 |   autoware_perception_msgs::DynamicObjectWithFeatureArray objects;
269 |   objects.header = message_header_;
270 |   cluster2d_->getObjects(confidence_thresh, height_thresh, min_pts_num, objects,
271 |                          message_header_);
272 | 
273 |   d_objects_pub_.publish(objects);
274 | 
275 |   if (viz_confidence_image_){
276 |     cv::Mat confidence_image = this->get_confidence_image(output);
277 |     nav_msgs::OccupancyGrid confidence_map =
278 |         this->get_confidence_map(confidence_image);
279 | 
280 |     confidence_image_pub_.publish(
281 |         cv_bridge::CvImage(message_header_, sensor_msgs::image_encodings::MONO8,
282 |                            confidence_image)
283 |         .toImageMsg());
284 |     confidence_map_pub_.publish(confidence_map);
285 |   }
286 | 
287 |   if (viz_class_image_){
288 |   cv::Mat class_image = this->get_class_image(output);
289 |   class_image_pub_.publish(
290 |       cv_bridge::CvImage(message_header_, sensor_msgs::image_encodings::RGB8,
291 |                          class_image)
292 |           .toImageMsg());
293 |   }
294 | 
295 |   if (pub_colored_points_){
296 |     pubColoredPoints(objects);
297 |   }
298 | }
299 | 


--------------------------------------------------------------------------------
/src/cluster2d.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright 2018-2019 Autoware Foundation. All rights reserved.
  3 |  *
  4 |  * Licensed under the Apache License, Version 2.0 (the "License");
  5 |  * you may not use this file except in compliance with the License.
  6 |  * You may obtain a copy of the License at
  7 |  *
  8 |  *     http://www.apache.org/licenses/LICENSE-2.0
  9 |  *
 10 |  * Unless required by applicable law or agreed to in writing, software
 11 |  * distributed under the License is distributed on an "AS IS" BASIS,
 12 |  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13 |  * See the License for the specific language governing permissions and
 14 |  * limitations under the License.
 15 |  */
 16 | #include "cluster2d.h"
 17 | 
 18 | bool Cluster2D::init(int rows, int cols, float range) {
 19 |   rows_ = rows;
 20 |   cols_ = cols;
 21 |   siz_ = rows * cols;
 22 |   range_ = range;
 23 |   scale_ = 0.5 * static_cast<float>(rows_) / range_;
 24 |   inv_res_x_ = 0.5 * static_cast<float>(cols_) / range_;
 25 |   inv_res_y_ = 0.5 * static_cast<float>(rows_) / range_;
 26 |   point2grid_.clear();
 27 |   id_img_.assign(siz_, -1);
 28 |   pc_ptr_.reset();
 29 |   valid_indices_in_pc_ = nullptr;
 30 |   return true;
 31 | }
 32 | 
 33 | void Cluster2D::traverse(Node *x) {
 34 |   std::vector<Node *> p;
 35 |   p.clear();
 36 | 
 37 |   // Cross until reaching center_node
 38 |   while (x->traversed == 0) {
 39 |     p.push_back(x);
 40 |     x->traversed = 2;
 41 |     x = x->center_node;
 42 |   }
 43 |   // x is center_node. Set is_center to true for all crossed nodes including x.
 44 |   if (x->traversed == 2) {
 45 |     for (int i = static_cast<int>(p.size()) - 1; i >= 0 && p[i] != x; i--) {
 46 |       p[i]->is_center = true;
 47 |     }
 48 |     x->is_center = true;
 49 |   }
 50 |   // Set all the parents of the traversed nodes to x
 51 |   for (size_t i = 0; i < p.size(); i++) {
 52 |     Node *y = p[i];
 53 |     y->traversed = 1;
 54 |     y->parent = x->parent;
 55 |   }
 56 | }
 57 | 
 58 | void Cluster2D::cluster(const float *output,
 59 |                         const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
 60 |                         const pcl::PointIndices &valid_indices,
 61 |                         float objectness_thresh,
 62 |                         bool use_all_grids_for_clustering) {
 63 |   const float *category_pt_data = output;
 64 |   const float *instance_pt_x_data = output + siz_;
 65 |   const float *instance_pt_y_data = output + siz_ * 2;
 66 | 
 67 |   pc_ptr_ = pc_ptr;
 68 | 
 69 |   std::vector<std::vector<Node>> nodes(rows_, std::vector<Node>(cols_, Node()));
 70 | 
 71 |   // map points into grids
 72 |   size_t tot_point_num = pc_ptr_->size();
 73 |   valid_indices_in_pc_ = &(valid_indices.indices);
 74 |   point2grid_.assign(valid_indices_in_pc_->size(), -1);
 75 | 
 76 |   for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) {
 77 |     int point_id = valid_indices_in_pc_->at(i);
 78 |     const auto &point = pc_ptr_->points[point_id];
 79 |     // * the coordinates of x and y have been exchanged in feature generation
 80 |     // step,
 81 |     // so we swap them back here.
 82 |     int pos_x = F2I(point.y, range_, inv_res_x_);  // col
 83 |     int pos_y = F2I(point.x, range_, inv_res_y_);  // row
 84 |     if (IsValidRowCol(pos_y, pos_x)) {
 85 |       // get grid index and count point number for corresponding node
 86 |       point2grid_[i] = RowCol2Grid(pos_y, pos_x);
 87 |       nodes[pos_y][pos_x].point_num++;
 88 |     }
 89 |   }
 90 | 
 91 |   // construct graph with center offset prediction and objectness
 92 |   for (int row = 0; row < rows_; ++row) {
 93 |     for (int col = 0; col < cols_; ++col) {
 94 |       int grid = RowCol2Grid(row, col);
 95 |       Node *node = &nodes[row][col];
 96 |       // DisjoinyMakeSet is x->parent = x; x->node_rank = 0; so x's parent is x.
 97 |       DisjointSetMakeSet(node);
 98 |       node->is_object =
 99 |           (use_all_grids_for_clustering || nodes[row][col].point_num > 0) &&
100 |           (*(category_pt_data + grid) >= objectness_thresh);
101 |       int center_row = std::round(row + instance_pt_x_data[grid] * scale_);
102 |       int center_col = std::round(col + instance_pt_y_data[grid] * scale_);
103 |       center_row = std::min(std::max(center_row, 0), rows_ - 1);
104 |       center_col = std::min(std::max(center_col, 0), cols_ - 1);
105 |       node->center_node = &nodes[center_row][center_col];
106 |     }
107 |   }
108 | 
109 |   // traverse nodes
110 |   for (int row = 0; row < rows_; ++row) {
111 |     for (int col = 0; col < cols_; ++col) {
112 |       Node *node = &nodes[row][col];
113 |       if (node->is_object && node->traversed == 0) {
114 |         traverse(node);
115 |       }
116 |     }
117 |   }
118 | 
119 |   // If there is a node with is_center somewhere in the four directions, connect
120 |   // the nodes
121 |   for (int row = 0; row < rows_; ++row) {
122 |     for (int col = 0; col < cols_; ++col) {
123 |       Node *node = &nodes[row][col];
124 |       if (!node->is_center) {
125 |         continue;
126 |       }
127 |       for (int row2 = row - 1; row2 <= row + 1; ++row2) {
128 |         for (int col2 = col - 1; col2 <= col + 1; ++col2) {
129 |           if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) {
130 |             Node *node2 = &nodes[row2][col2];
131 |             if (node2->is_center) {
132 |               // Same if root is different
133 |               DisjointSetUnion(node, node2);
134 |             }
135 |           }
136 |         }
137 |       }
138 |     }
139 |   }
140 | 
141 |   int count_obstacles = 0;
142 |   obstacles_.clear();
143 |   id_img_.assign(siz_, -1);
144 |   for (int row = 0; row < rows_; ++row) {
145 |     for (int col = 0; col < cols_; ++col) {
146 |       Node *node = &nodes[row][col];
147 |       if (!node->is_object) {
148 |         continue;
149 |       }
150 |       Node *root = DisjointSetFind(node);
151 |       if (root->obstacle_id < 0) {
152 |         root->obstacle_id = count_obstacles++;
153 |         obstacles_.push_back(Obstacle());
154 |       }
155 |       int grid = RowCol2Grid(row, col);
156 |       id_img_[grid] = root->obstacle_id;
157 |       obstacles_[root->obstacle_id].grids.push_back(grid);
158 |     }
159 |   }
160 | }
161 | 
162 | void Cluster2D::filter(const float *output) {
163 |   const float *confidence_pt_data = output + siz_ * 3;
164 |   const float *height_pt_data = output + siz_ * 11;
165 | 
166 |   for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
167 |     Obstacle *obs = &obstacles_[obstacle_id];
168 |     double score = 0.0;
169 |     double height = 0.0;
170 |     for (int grid : obs->grids) {
171 |       score += static_cast<double>(confidence_pt_data[grid]);
172 |       height += static_cast<double>(height_pt_data[grid]);
173 |     }
174 |     obs->score = score / static_cast<double>(obs->grids.size());
175 |     obs->height = height / static_cast<double>(obs->grids.size());
176 |     obs->cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
177 |   }
178 | }
179 | 
180 | void Cluster2D::classify(const float *output) {
181 |   const float *classify_pt_data = output + siz_ * 4;
182 |   int num_classes = 6;
183 |   for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) {
184 |     Obstacle *obs = &obstacles_[obs_id];
185 | 
186 |     for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) {
187 |       int grid = obs->grids[grid_id];
188 |       for (int k = 0; k < num_classes; k++) {
189 |         obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid];
190 |       }
191 |     }
192 |     int meta_type_id = 0;
193 |     for (int k = 0; k < num_classes; k++) {
194 |       obs->meta_type_probs[k] /= obs->grids.size();
195 |       if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) {
196 |         meta_type_id = k;
197 |       }
198 |     }
199 |     obs->meta_type = static_cast<MetaType>(meta_type_id);
200 |   }
201 | }
202 | 
203 | autoware_perception_msgs::DynamicObjectWithFeature Cluster2D::obstacleToObject(
204 |     const Obstacle &in_obstacle, const std_msgs::Header &in_header) {
205 |   autoware_perception_msgs::DynamicObjectWithFeature resulting_object;
206 | 
207 |   sensor_msgs::PointCloud2 ros_pc;
208 |   pcl::PointCloud<pcl::PointXYZI> in_cluster = *in_obstacle.cloud_ptr;
209 |   pcl::toROSMsg(in_cluster, ros_pc);
210 | 
211 |   resulting_object.feature.cluster = ros_pc;
212 |   resulting_object.feature.cluster.header = in_header;
213 |   resulting_object.object.semantic.confidence = in_obstacle.score;
214 |   const std::string label = in_obstacle.GetTypeString();
215 |   if (label == "person") {
216 |     resulting_object.object.semantic.type =
217 |         resulting_object.object.semantic.PEDESTRIAN;
218 |   } else if (label == "bike" || label == "bicycle") {
219 |     resulting_object.object.semantic.type =
220 |         resulting_object.object.semantic.BICYCLE;
221 |   } else if (label == "car") {
222 |     resulting_object.object.semantic.type =
223 |         resulting_object.object.semantic.CAR;
224 |   } else if (label == "truck") {
225 |     resulting_object.object.semantic.type =
226 |         resulting_object.object.semantic.TRUCK;
227 |   } else if (label == "bus") {
228 |     resulting_object.object.semantic.type =
229 |         resulting_object.object.semantic.BUS;
230 |   } else {
231 |     // d_object.object.semantic.type = d_object.object.semantic.UNKNOWN;
232 |     resulting_object.object.semantic.type =
233 |         resulting_object.object.semantic.PEDESTRIAN;
234 |   }
235 | 
236 |   float min_x = std::numeric_limits<float>::max();
237 |   float max_x = -std::numeric_limits<float>::max();
238 |   float min_y = std::numeric_limits<float>::max();
239 |   float max_y = -std::numeric_limits<float>::max();
240 |   float min_z = std::numeric_limits<float>::max();
241 |   float max_z = -std::numeric_limits<float>::max();
242 |   float average_x = 0, average_y = 0, average_z = 0;
243 |   float length, width, height;
244 | 
245 |   pcl::PointXYZ min_point;
246 |   pcl::PointXYZ max_point;
247 |   pcl::PointXYZ average_point;
248 |   pcl::PointXYZ centroid;
249 | 
250 |   for (auto pit = in_cluster.points.begin(); pit != in_cluster.points.end();
251 |        ++pit) {
252 |     average_x += pit->x;
253 |     average_y += pit->y;
254 |     average_z += pit->z;
255 |     centroid.x += pit->x;
256 |     centroid.y += pit->y;
257 |     centroid.z += pit->z;
258 | 
259 |     if (pit->x < min_x) min_x = pit->x;
260 |     if (pit->y < min_y) min_y = pit->y;
261 |     if (pit->z < min_z) min_z = pit->z;
262 |     if (pit->x > max_x) max_x = pit->x;
263 |     if (pit->y > max_y) max_y = pit->y;
264 |     if (pit->z > max_z) max_z = pit->z;
265 |   }
266 |   // min, max points
267 |   min_point.x = min_x;
268 |   min_point.y = min_y;
269 |   min_point.z = min_z;
270 |   max_point.x = max_x;
271 |   max_point.y = max_y;
272 |   max_point.z = max_z;
273 | 
274 |   if (in_cluster.points.size() > 0) {
275 |     centroid.x /= in_cluster.points.size();
276 |     centroid.y /= in_cluster.points.size();
277 |     centroid.z /= in_cluster.points.size();
278 |   }
279 |   length = max_point.x - min_point.x;
280 |   width = max_point.y - min_point.y;
281 |   height = max_point.z - min_point.z;
282 | 
283 |   resulting_object.object.state.pose_covariance.pose.position.x =
284 |       min_point.x + length / 2;
285 |   resulting_object.object.state.pose_covariance.pose.position.y =
286 |       min_point.y + width / 2;
287 |   resulting_object.object.state.pose_covariance.pose.position.z =
288 |       min_point.z + height / 2;
289 | 
290 |   std::vector<cv::Point2f> points;
291 |   for (unsigned int i = 0; i < in_cluster.points.size(); i++) {
292 |     cv::Point2f pt;
293 |     pt.x = in_cluster.points[i].x;
294 |     pt.y = in_cluster.points[i].y;
295 |     points.push_back(pt);
296 |   }
297 | 
298 |   // resulting_object.space_frame = in_header.frame_id;
299 | 
300 |   return resulting_object;
301 | }
302 | 
303 | void Cluster2D::getObjects(
304 |     const float confidence_thresh, const float height_thresh,
305 |     const int min_pts_num,
306 |     autoware_perception_msgs::DynamicObjectWithFeatureArray &objects,
307 |     const std_msgs::Header &in_header) {
308 |   for (size_t i = 0; i < point2grid_.size(); ++i) {
309 |     int grid = point2grid_[i];
310 |     if (grid < 0) {
311 |       continue;
312 |     }
313 | 
314 |     int obstacle_id = id_img_[grid];
315 | 
316 |     int point_id = valid_indices_in_pc_->at(i);
317 | 
318 |     if (obstacle_id >= 0 &&
319 |         obstacles_[obstacle_id].score >= confidence_thresh) {
320 |       if (height_thresh < 0 ||
321 |           pc_ptr_->points[point_id].z <=
322 |               obstacles_[obstacle_id].height + height_thresh) {
323 |         obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]);
324 |       }
325 |     }
326 |   }
327 | 
328 |   for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
329 |     Obstacle *obs = &obstacles_[obstacle_id];
330 |     if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num) {
331 |       continue;
332 |     }
333 |     autoware_perception_msgs::DynamicObjectWithFeature out_obj =
334 |         obstacleToObject(*obs, in_header);
335 |     objects.feature_objects.push_back(out_obj);
336 |   }
337 | }
338 | 
339 | ObjectType getObjectType(const MetaType meta_type_id) {
340 |   switch (meta_type_id) {
341 |     case META_UNKNOWN:
342 |       return UNKNOWN;
343 |     case META_SMALLMOT:
344 |       return VEHICLE;
345 |     case META_BIGMOT:
346 |       return VEHICLE;
347 |     case META_NONMOT:
348 |       return BICYCLE;
349 |     case META_PEDESTRIAN:
350 |       return PEDESTRIAN;
351 |     default: { return UNKNOWN; }
352 |   }
353 | }
354 | 


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