| 75 | } |
| 76 | |
| 77 | Cloud::Ptr SegNode::trellisCloud(const std::vector<std::vector<TreeVertex>> &landmarks) |
| 78 | { |
| 79 | CloudT::Ptr vtxCloud = CloudT::Ptr(new CloudT); |
| 80 | std::vector<float> color_values((int)landmarks.size()); |
| 81 | std::iota(std::begin(color_values), std::end(color_values), 1); |
| 82 | std::random_device rd; |
| 83 | std::mt19937 gen(rd()); |
| 84 | std::shuffle(color_values.begin(), color_values.end(), gen); |
| 85 | int color_id = 0; |
| 86 | |
| 87 | for (auto landmark : landmarks) |
| 88 | { |
| 89 | for (auto vtx : landmark) |
| 90 | { |
| 91 | for (auto point : vtx.points) |
| 92 | { |
| 93 | point.intensity = color_values[color_id]; |
| 94 | vtxCloud->points.push_back(point); |
| 95 | } |
| 96 | } |
| 97 | color_id++; |
| 98 | } |
| 99 | vtxCloud->height = 1; |
| 100 | vtxCloud->width = vtxCloud->points.size(); |
| 101 | return vtxCloud; |
| 102 | } |
| 103 | |
| 104 | void SegNode::SegCb_(const sensor_msgs::PointCloud2ConstPtr &cloudMsg) |
| 105 | { |
nothing calls this directly
no outgoing calls
no test coverage detected