| 184 | } |
| 185 | |
| 186 | bool SLOAMNode::run(const SE3 initialGuess, const SE3 prevKeyPose, CloudT::Ptr cloud, ros::Time stamp, SE3 &outPose) |
| 187 | { |
| 188 | |
| 189 | SloamInput sloamIn = SloamInput(); |
| 190 | SloamOutput sloamOut = SloamOutput(); |
| 191 | |
| 192 | SE3 poseEstimate = prevKeyPose * initialGuess; |
| 193 | // sloamIn.initialGuess = initialGuess; |
| 194 | // sloamIn.prevPose = mapCurrPose_; |
| 195 | sloamIn.poseEstimate = poseEstimate; |
| 196 | sloamIn.distance = initialGuess.translation().norm(); |
| 197 | semanticMap_.getSubmap(poseEstimate, sloamIn.mapModels); |
| 198 | |
| 199 | if (!firstScan_ && sloamIn.mapModels.size() == 0) |
| 200 | { |
| 201 | ROS_DEBUG("Discarding msg"); |
| 202 | return false; |
| 203 | } |
| 204 | |
| 205 | ROS_INFO_STREAM("Entering Callback. Lidar data stamp: " << stamp); |
| 206 | |
| 207 | // RUN SEGMENTATION |
| 208 | cv::Mat rMask = cv::Mat::zeros(cloud->height, cloud->width, CV_8U); |
| 209 | segmentator_->run(cloud, rMask); |
| 210 | |
| 211 | CloudT::Ptr groundCloud(new CloudT()); |
| 212 | segmentator_->maskCloud(cloud, rMask, groundCloud, 1); |
| 213 | |
| 214 | CloudT::Ptr treeCloud(new CloudT); |
| 215 | segmentator_->maskCloud(cloud, rMask, treeCloud, 255, true); |
| 216 | |
| 217 | groundCloud->header = cloud->header; |
| 218 | sloamIn.groundCloud = groundCloud; |
| 219 | ROS_DEBUG_STREAM("Num ground features available: " << groundCloud->width); |
| 220 | |
| 221 | // Trellis graph instance segmentation |
| 222 | graphDetector_.computeGraph(cloud, treeCloud, sloamIn.landmarks); |
| 223 | ROS_INFO_STREAM("Num Landmarks detected with Trellis: " << sloamIn.landmarks.size()); |
| 224 | ROS_INFO_STREAM("Num Map Landmarks: " << sloamIn.mapModels.size()); |
| 225 | |
| 226 | if (debugMode_ && !firstScan_) |
| 227 | { |
| 228 | // DEBUG TOPICS |
| 229 | auto mapGFeats = getPrevGroundFeatures(); |
| 230 | mapGFeats.header.frame_id = map_frame_id_; |
| 231 | pubMapGroundFeatures_.publish(mapGFeats); |
| 232 | pubMapGroundModel_.publish(vizGroundModel(getPrevGroundModel(), map_frame_id_, 444)); |
| 233 | } |
| 234 | |
| 235 | bool success = RunSloam(sloamIn, sloamOut); |
| 236 | semanticMap_.updateMap(sloamOut.tm, sloamOut.matches); |
| 237 | ROS_DEBUG_STREAM("Publishing Results. Success? " << success << std::endl); |
| 238 | |
| 239 | if (success) |
| 240 | { |
| 241 | if (firstScan_ && sloamOut.tm.size() > 0) |
| 242 | firstScan_ = false; |
| 243 |
no test coverage detected