| 636 | } |
| 637 | |
| 638 | void RosBaseDataProvider::publishFrontendStats( |
| 639 | const SpinOutputPacket& vio_output) const { |
| 640 | // Get frontend data for resiliency output |
| 641 | const DebugTrackerInfo& debug_tracker_info = vio_output.getTrackerInfo(); |
| 642 | |
| 643 | // Create message type |
| 644 | std_msgs::Float64MultiArray frontend_stats_msg; |
| 645 | |
| 646 | // Build Message Layout |
| 647 | frontend_stats_msg.data.resize(13); |
| 648 | frontend_stats_msg.data[0] = debug_tracker_info.nrDetectedFeatures_; |
| 649 | frontend_stats_msg.data[1] = debug_tracker_info.nrTrackerFeatures_; |
| 650 | frontend_stats_msg.data[2] = debug_tracker_info.nrMonoInliers_; |
| 651 | frontend_stats_msg.data[3] = debug_tracker_info.nrMonoPutatives_; |
| 652 | frontend_stats_msg.data[4] = debug_tracker_info.nrStereoInliers_; |
| 653 | frontend_stats_msg.data[5] = debug_tracker_info.nrStereoPutatives_; |
| 654 | frontend_stats_msg.data[6] = debug_tracker_info.monoRansacIters_; |
| 655 | frontend_stats_msg.data[7] = debug_tracker_info.stereoRansacIters_; |
| 656 | frontend_stats_msg.data[8] = debug_tracker_info.nrValidRKP_; |
| 657 | frontend_stats_msg.data[9] = debug_tracker_info.nrNoLeftRectRKP_; |
| 658 | frontend_stats_msg.data[10] = debug_tracker_info.nrNoRightRectRKP_; |
| 659 | frontend_stats_msg.data[11] = debug_tracker_info.nrNoDepthRKP_; |
| 660 | frontend_stats_msg.data[12] = debug_tracker_info.nrFailedArunRKP_; |
| 661 | frontend_stats_msg.layout.dim.resize(1); |
| 662 | frontend_stats_msg.layout.dim[0].size = frontend_stats_msg.data.size(); |
| 663 | frontend_stats_msg.layout.dim[0].stride = 1; |
| 664 | frontend_stats_msg.layout.dim[0].label = |
| 665 | "FrontEnd: nrDetFeat, nrTrackFeat, nrMoIn, nrMoPu, nrStIn, nrStPu, " |
| 666 | "moRaIt, stRaIt, nrVaRKP, nrNoLRKP, nrNoRRKP, nrNoDRKP nrFaARKP"; |
| 667 | |
| 668 | // Publish Message |
| 669 | frontend_stats_pub_.publish(frontend_stats_msg); |
| 670 | } |
| 671 | |
| 672 | void RosBaseDataProvider::publishResiliency( |
| 673 | const SpinOutputPacket& vio_output) const { |