| 156 | } |
| 157 | |
| 158 | Cloud::Ptr SLOAMNode::trellisCloud(const std::vector<std::vector<TreeVertex>> &landmarks) |
| 159 | { |
| 160 | CloudT::Ptr vtxCloud = CloudT::Ptr(new CloudT); |
| 161 | std::vector<float> color_values((int)landmarks.size()); |
| 162 | std::iota(std::begin(color_values), std::end(color_values), 1); |
| 163 | std::random_device rd; |
| 164 | std::mt19937 gen(rd()); |
| 165 | std::shuffle(color_values.begin(), color_values.end(), gen); |
| 166 | int color_id = 0; |
| 167 | |
| 168 | for (auto landmark : landmarks) |
| 169 | { |
| 170 | for (auto vtx : landmark) |
| 171 | { |
| 172 | for (auto point : vtx.points) |
| 173 | { |
| 174 | point.intensity = color_values[color_id]; |
| 175 | vtxCloud->points.push_back(point); |
| 176 | } |
| 177 | } |
| 178 | color_id++; |
| 179 | } |
| 180 | vtxCloud->height = 1; |
| 181 | vtxCloud->width = vtxCloud->points.size(); |
| 182 | vtxCloud->header.frame_id = map_frame_id_; |
| 183 | return vtxCloud; |
| 184 | } |
| 185 | |
| 186 | bool SLOAMNode::run(const SE3 initialGuess, const SE3 prevKeyPose, CloudT::Ptr cloud, ros::Time stamp, SE3 &outPose) |
| 187 | { |
nothing calls this directly
no outgoing calls
no test coverage detected