Method SegNode

sloam/src/segmentation/inferenceNode.cpp:36–75

};

SegNode::SegNode(const ros::NodeHandle &nh) : nh_(nh)
{

  std::string cloud_topic;
  nh_.param<std::string>("cloud_topic", cloud_topic, "/os_cloud_node/cloud_undistort");
  cloudSub_ = nh_.subscribe(cloud_topic, 10, &SegNode::SegCb_, this);

  treePub_ = nh_.advertise<CloudT>("segmentation/tree", 1);
  groundPub_ = nh_.advertise<CloudT>("segmentation/ground", 1);
  trellisPub_ = nh_.advertise<CloudT>("segmentation/trellis", 1);

  std::string modelFilepath;
  nh_.getParam("seg_model_path", modelFilepath);
  std::cout << "model: " << modelFilepath << std::endl;
  float fov = nh_.param("seg_lidar_fov", 22.5);
  int lidar_w = nh_.param("seg_lidar_w", 2048);
  int lidar_h = nh_.param("seg_lidar_h", 64);
  bool do_destagger = nh_.param("do_destagger", true);

  auto temp_seg = boost::make_shared<seg::Segmentation>(modelFilepath, fov, -fov, lidar_w, lidar_h, 1, do_destagger);
  segmentator_ = std::move(temp_seg);
  prevStamp = ros::Time::now();

  float beam_cluster_threshold = nh_.param("beam_cluster_threshold", 0.1);
  int min_vertex_size = nh_.param("min_vertex_size", 2);
  float max_dist_to_centroid = nh_.param("max_dist_to_centroid", 0.2);
  int min_landmark_size = nh_.param("min_landmark_size", 4);
  float min_landmark_height = nh_.param("min_landmark_height", 1);

  Instance::Params params;
  params.beam_cluster_threshold = beam_cluster_threshold;
  params.min_vertex_size = min_vertex_size;
  params.max_dist_to_centroid = max_dist_to_centroid;
  params.min_landmark_size = min_landmark_size;
  params.min_landmark_height = min_landmark_height;

  graphDetector_ = Instance();
  graphDetector_.set_params(params);
  graphDetector_.reset_tree_id();
}

Cloud::Ptr SegNode::trellisCloud(const std::vector<std::vector<TreeVertex>> &landmarks)
{