Skip to content

Commit

Permalink
refactor(lidar_apollo_segmentation_tvm/_nodes): remove autoware_auto_…
Browse files Browse the repository at this point in the history
…common dependency (autowarefoundation#3136)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: M. Fatih Cırıt <mfc@leodrive.ai>
Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>
  • Loading branch information
xmfcx authored and Mingyu1991 committed Jun 26, 2023
1 parent ca1e376 commit 298a32f
Show file tree
Hide file tree
Showing 16 changed files with 151 additions and 197 deletions.
7 changes: 4 additions & 3 deletions common/tvm_utility/include/tvm_utility/pipeline.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <tvm_vendor/tvm/runtime/packed_func.h>
#include <tvm_vendor/tvm/runtime/registry.h>

#include <cstdint>
#include <fstream>
#include <memory>
#include <string>
Expand Down Expand Up @@ -200,7 +201,7 @@ typedef struct
typedef struct
{
// Network info
std::array<char, 3> modelzoo_version;
std::array<int8_t, 3> modelzoo_version;
std::string network_name;
std::string network_backend;

Expand Down Expand Up @@ -320,7 +321,7 @@ class InferenceEngineTVM : public InferenceEngine
* @param[in] version_from Earliest supported model version.
* @return The version status.
*/
Version version_check(const std::array<char, 3> & version_from) const
Version version_check(const std::array<int8_t, 3> & version_from) const
{
auto x{config_.modelzoo_version[0]};
auto y{config_.modelzoo_version[1]};
Expand All @@ -344,7 +345,7 @@ class InferenceEngineTVM : public InferenceEngine
tvm::runtime::PackedFunc execute;
tvm::runtime::PackedFunc get_output;
// Latest supported model version.
const std::array<char, 3> version_up_to{2, 1, 0};
const std::array<int8_t, 3> version_up_to{2, 1, 0};
};

template <
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_
#define LIDAR_APOLLO_SEGMENTATION_TVM__CLUSTER2D_HPP_

#include <common/types.hpp>
#include <lidar_apollo_segmentation_tvm/disjoint_set.hpp>
#include <lidar_apollo_segmentation_tvm/util.hpp>
#include <lidar_apollo_segmentation_tvm/visibility_control.hpp>
Expand All @@ -28,6 +27,7 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <cstdint>
#include <memory>
#include <vector>

Expand All @@ -37,9 +37,6 @@ namespace perception
{
namespace lidar_apollo_segmentation_tvm
{
using autoware::common::types::bool8_t;
using autoware::common::types::char8_t;
using autoware::common::types::float32_t;

/// \brief Internal obstacle classification categories.
enum class MetaType {
Expand All @@ -56,11 +53,11 @@ struct Obstacle
{
std::vector<int32_t> grids;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr;
float32_t score;
float32_t height;
float32_t heading;
float score;
float height;
float heading;
MetaType meta_type;
std::vector<float32_t> meta_type_probs;
std::vector<float> meta_type_probs;

Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN)
{
Expand All @@ -78,7 +75,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D
/// \param[in] rows The number of rows in the cluster.
/// \param[in] cols The number of columns in the cluster.
/// \param[in] range Scaling factor.
explicit Cluster2D(int32_t rows, int32_t cols, float32_t range);
explicit Cluster2D(int32_t rows, int32_t cols, float range);

/// \brief Construct a directed graph and search the connected components for candidate object
/// clusters.
Expand All @@ -88,17 +85,17 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D
/// \param[in] objectness_thresh Threshold for filtering out non-object cells.
/// \param[in] use_all_grids_for_clustering
void cluster(
const float32_t * inferred_data, const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & pc_ptr,
const pcl::PointIndices & valid_indices, float32_t objectness_thresh,
bool8_t use_all_grids_for_clustering);
const float * inferred_data, const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & pc_ptr,
const pcl::PointIndices & valid_indices, float objectness_thresh,
bool use_all_grids_for_clustering);

/// \brief Populate the fields of obstacles_ elements.
/// \param[in] inferred_data Prediction information from the neural network inference.
void filter(const float32_t * inferred_data);
void filter(const float * inferred_data);

/// \brief Assign a classification type to the obstacles_ elements.
/// \param[in] inferred_data Prediction information from the neural network inference.
void classify(const float32_t * inferred_data);
void classify(const float * inferred_data);

/// \brief Remove the candidate clusters that don't meet the parameters' requirements.
/// \param[in] confidence_thresh The detection confidence score threshold.
Expand All @@ -107,7 +104,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D
/// \param[in] min_pts_num The candidate clusters with less than min_pts_num points are removed.
/// \return The detected objects.
std::shared_ptr<tier4_perception_msgs::msg::DetectedObjectsWithFeature> getObjects(
float32_t confidence_thresh, float32_t height_thresh, int32_t min_pts_num);
float confidence_thresh, float height_thresh, int32_t min_pts_num);

/// \brief Transform an obstacle from the internal representation to the external one.
/// \param[in] in_obstacle
Expand All @@ -119,10 +116,10 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D
const int32_t rows_;
const int32_t cols_;
const int32_t siz_;
const float32_t range_;
const float32_t scale_;
const float32_t inv_res_x_;
const float32_t inv_res_y_;
const float range_;
const float scale_;
const float inv_res_x_;
const float inv_res_y_;
std::vector<int32_t> point2grid_;
std::vector<Obstacle> obstacles_;
std::vector<int32_t> id_img_;
Expand All @@ -135,10 +132,10 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL Cluster2D
{
Node * center_node;
Node * parent;
char8_t node_rank;
char8_t traversed;
bool8_t is_center;
bool8_t is_object;
int8_t node_rank;
int8_t traversed;
bool is_center;
bool is_object;
int32_t point_num;
int32_t obstacle_id;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_
#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_GENERATOR_HPP_

#include <common/types.hpp>
#include <lidar_apollo_segmentation_tvm/feature_map.hpp>
#include <lidar_apollo_segmentation_tvm/util.hpp>
#include <lidar_apollo_segmentation_tvm/visibility_control.hpp>
Expand All @@ -33,17 +32,15 @@ namespace perception
{
namespace lidar_apollo_segmentation_tvm
{
using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;

/// \brief A FeatureMap generator based on channel feature information.
class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator
{
private:
const bool8_t use_intensity_feature_;
const bool8_t use_constant_feature_;
const float32_t min_height_;
const float32_t max_height_;
const bool use_intensity_feature_;
const bool use_constant_feature_;
const float min_height_;
const float max_height_;
std::shared_ptr<FeatureMapInterface> map_ptr_;

public:
Expand All @@ -56,8 +53,8 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL FeatureGenerator
/// \param[in] min_height The minimum height.
/// \param[in] max_height The maximum height.
explicit FeatureGenerator(
int32_t width, int32_t height, int32_t range, bool8_t use_intensity_feature,
bool8_t use_constant_feature, float32_t min_height, float32_t max_height);
int32_t width, int32_t height, int32_t range, bool use_intensity_feature,
bool use_constant_feature, float min_height, float max_height);

/// \brief Generate a FeatureMap based on the configured features of this object.
/// \param[in] pc_ptr Pointcloud used to populate the generated FeatureMap.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@
#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_
#define LIDAR_APOLLO_SEGMENTATION_TVM__FEATURE_MAP_HPP_

#include <common/types.hpp>

#include <memory>
#include <vector>

Expand All @@ -26,7 +24,6 @@ namespace perception
{
namespace lidar_apollo_segmentation_tvm
{
using autoware::common::types::float32_t;

/// \brief Abstract interface for FeatureMap.
struct FeatureMapInterface
Expand All @@ -36,50 +33,50 @@ struct FeatureMapInterface
const int32_t width;
const int32_t height;
const int32_t range;
float32_t * max_height_data; // channel 0
float32_t * mean_height_data; // channel 1
float32_t * count_data; // channel 2
float32_t * direction_data; // channel 3
float32_t * top_intensity_data; // channel 4
float32_t * mean_intensity_data; // channel 5
float32_t * distance_data; // channel 6
float32_t * nonempty_data; // channel 7
std::vector<float32_t> map_data;
virtual void initializeMap(std::vector<float32_t> & map) = 0;
virtual void resetMap(std::vector<float32_t> & map) = 0;
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;
explicit FeatureMapInterface(int32_t _channels, int32_t _width, int32_t _height, int32_t _range);
};

/// \brief FeatureMap with no extra feature channels.
struct FeatureMap : public FeatureMapInterface
{
explicit FeatureMap(int32_t width, int32_t height, int32_t range);
void initializeMap(std::vector<float32_t> & map) override;
void resetMap(std::vector<float32_t> & map) override;
void initializeMap(std::vector<float> & map) override;
void resetMap(std::vector<float> & map) override;
};

/// \brief FeatureMap with an intensity feature channel.
struct FeatureMapWithIntensity : public FeatureMapInterface
{
explicit FeatureMapWithIntensity(int32_t width, int32_t height, int32_t range);
void initializeMap(std::vector<float32_t> & map) override;
void resetMap(std::vector<float32_t> & map) override;
void initializeMap(std::vector<float> & map) override;
void resetMap(std::vector<float> & map) override;
};

/// \brief FeatureMap with a constant feature channel.
struct FeatureMapWithConstant : public FeatureMapInterface
{
explicit FeatureMapWithConstant(int32_t width, int32_t height, int32_t range);
void initializeMap(std::vector<float32_t> & map) override;
void resetMap(std::vector<float32_t> & map) override;
void initializeMap(std::vector<float> & map) override;
void resetMap(std::vector<float> & map) override;
};

/// \brief FeatureMap with constant and intensity feature channels.
struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface
{
explicit FeatureMapWithConstantAndIntensity(int32_t width, int32_t height, int32_t range);
void initializeMap(std::vector<float32_t> & map) override;
void resetMap(std::vector<float32_t> & map) override;
void initializeMap(std::vector<float> & map) override;
void resetMap(std::vector<float> & map) override;
};
} // namespace lidar_apollo_segmentation_tvm
} // namespace perception
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_
#define LIDAR_APOLLO_SEGMENTATION_TVM__LIDAR_APOLLO_SEGMENTATION_TVM_HPP_

#include <common/types.hpp>
#include <lidar_apollo_segmentation_tvm/cluster2d.hpp>
#include <lidar_apollo_segmentation_tvm/feature_generator.hpp>
#include <lidar_apollo_segmentation_tvm/visibility_control.hpp>
Expand All @@ -36,6 +35,7 @@
#include <tvm_vendor/tvm/runtime/packed_func.h>
#include <tvm_vendor/tvm/runtime/registry.h>

#include <cstdint>
#include <memory>
#include <string>
#include <vector>
Expand All @@ -46,8 +46,7 @@ namespace perception
{
namespace lidar_apollo_segmentation_tvm
{
using autoware::common::types::bool8_t;
using autoware::common::types::float32_t;

using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
using tvm_utility::pipeline::TVMArrayContainer;
using tvm_utility::pipeline::TVMArrayContainerVector;
Expand All @@ -66,8 +65,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPreProcessor
/// \param[in] max_height The maximum height.
explicit ApolloLidarSegmentationPreProcessor(
const tvm_utility::pipeline::InferenceEngineTVMConfig & config, int32_t range,
bool8_t use_intensity_feature, bool8_t use_constant_feature, float32_t min_height,
float32_t max_height);
bool use_intensity_feature, bool use_constant_feature, float min_height, float max_height);

/// \brief Transfer the input data to a TVM array.
/// \param[in] pc_ptr Input pointcloud.
Expand Down Expand Up @@ -103,8 +101,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor
explicit ApolloLidarSegmentationPostProcessor(
const tvm_utility::pipeline::InferenceEngineTVMConfig & config,
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & pc_ptr, int32_t range,
float32_t objectness_thresh, float32_t score_threshold, float32_t height_thresh,
int32_t min_pts_num);
float objectness_thresh, float score_threshold, float height_thresh, int32_t min_pts_num);

/// \brief Copy the inference result.
/// \param[in] input The result of the inference engine.
Expand All @@ -116,9 +113,9 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL ApolloLidarSegmentationPostProcessor
const int64_t output_width;
const int64_t output_height;
const int64_t output_datatype_bytes;
const float32_t objectness_thresh_;
const float32_t score_threshold_;
const float32_t height_thresh_;
const float objectness_thresh_;
const float score_threshold_;
const float height_thresh_;
const int32_t min_pts_num_;
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr pc_ptr_;
const std::shared_ptr<Cluster2D> cluster2d_;
Expand All @@ -145,9 +142,9 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation
/// object height by height_thresh are filtered out in the
/// post-processing step.
explicit ApolloLidarSegmentation(
int32_t range, float32_t score_threshold, bool8_t use_intensity_feature,
bool8_t use_constant_feature, float32_t z_offset, float32_t min_height, float32_t max_height,
float32_t objectness_thresh, int32_t min_pts_num, float32_t height_thresh);
int32_t range, float score_threshold, bool use_intensity_feature, bool use_constant_feature,
float z_offset, float min_height, float max_height, float objectness_thresh,
int32_t min_pts_num, float height_thresh);

/// \brief Detect obstacles.
/// \param[in] input Input pointcloud.
Expand All @@ -167,14 +164,14 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation

private:
const int32_t range_;
const float32_t score_threshold_;
const float32_t z_offset_;
const float32_t objectness_thresh_;
const float score_threshold_;
const float z_offset_;
const float objectness_thresh_;
const int32_t min_pts_num_;
const float32_t height_thresh_;
const float height_thresh_;
const pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud_ptr_;
// Earliest supported model version.
const std::array<char8_t, 3> model_version_from{2, 0, 0};
const std::array<int8_t, 3> model_version_from{2, 0, 0};

// Pipeline
using PrePT = ApolloLidarSegmentationPreProcessor;
Expand All @@ -194,7 +191,7 @@ class LIDAR_APOLLO_SEGMENTATION_TVM_PUBLIC ApolloLidarSegmentation
/// \throw tf2::TransformException If the pointcloud transformation fails.
void LIDAR_APOLLO_SEGMENTATION_TVM_LOCAL transformCloud(
const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud,
float32_t z_offset);
float z_offset);

rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
std::unique_ptr<tf2_ros::Buffer> tf_buffer_ = std::make_unique<tf2_ros::Buffer>(clock_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,20 +15,17 @@
#ifndef LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_
#define LIDAR_APOLLO_SEGMENTATION_TVM__LOG_TABLE_HPP_

#include <common/types.hpp>

namespace autoware
{
namespace perception
{
namespace lidar_apollo_segmentation_tvm
{
using autoware::common::types::float32_t;

/// \brief Use a lookup table to compute the natural logarithm of 1+num.
/// \param[in] num
/// \return ln(1+num)
float32_t calcApproximateLog(float32_t num);
float calcApproximateLog(float num);
} // namespace lidar_apollo_segmentation_tvm
} // namespace perception
} // namespace autoware
Expand Down
Loading

0 comments on commit 298a32f

Please sign in to comment.