| 670 | } |
| 671 | |
| 672 | void RosBaseDataProvider::publishResiliency( |
| 673 | const SpinOutputPacket& vio_output) const { |
| 674 | // Get frontend and velocity covariance data for resiliency output |
| 675 | const DebugTrackerInfo& debug_tracker_info = vio_output.getTrackerInfo(); |
| 676 | const gtsam::Matrix3& vel_cov = vio_output.getEstimatedVelCov(); |
| 677 | const gtsam::Matrix6& pose_cov = vio_output.getEstimatedPoseCov(); |
| 678 | |
| 679 | // Create message type for quality of SparkVIO |
| 680 | std_msgs::Float64MultiArray resiliency_msg; |
| 681 | |
| 682 | // Publishing extra information: |
| 683 | // cov_v_det and nrStIn should be the most relevant! |
| 684 | resiliency_msg.layout.dim[0].label = |
| 685 | "Values: cbrtPDet, cbrtVDet, nrStIn, nrMoIn. " |
| 686 | "Thresholds : cbrtPDet, cbrtVDet, nrStIn, nrMoIn."; |
| 687 | |
| 688 | CHECK_EQ(pose_cov.size(), 36); |
| 689 | gtsam::Matrix3 position_cov = gtsam::sub(pose_cov, 3, 6, 3, 6); |
| 690 | CHECK_EQ(position_cov.size(), 9); |
| 691 | |
| 692 | // Compute eigenvalues and determinant of velocity covariance |
| 693 | gtsam::Matrix U; |
| 694 | gtsam::Matrix V; |
| 695 | gtsam::Vector cov_v_eigv; |
| 696 | gtsam::svd(vel_cov, U, cov_v_eigv, V); |
| 697 | CHECK_EQ(cov_v_eigv.size(), 3); |
| 698 | |
| 699 | // Compute eigenvalues and determinant of position covariance |
| 700 | gtsam::Vector cov_p_eigv; |
| 701 | gtsam::svd(position_cov, U, cov_p_eigv, V); |
| 702 | CHECK_EQ(cov_p_eigv.size(), 3); |
| 703 | |
| 704 | // Quality statistics to publish |
| 705 | resiliency_msg.data.resize(8); |
| 706 | resiliency_msg.data[0] = |
| 707 | std::cbrt(cov_p_eigv(0) * cov_p_eigv(1) * cov_p_eigv(2)); |
| 708 | resiliency_msg.data[1] = |
| 709 | std::cbrt(cov_v_eigv(0) * cov_v_eigv(1) * cov_v_eigv(2)); |
| 710 | resiliency_msg.data[2] = debug_tracker_info.nrStereoInliers_; |
| 711 | resiliency_msg.data[3] = debug_tracker_info.nrMonoInliers_; |
| 712 | |
| 713 | // Publish thresholds for statistics |
| 714 | float pos_det_threshold, vel_det_threshold; |
| 715 | int mono_ransac_theshold, stereo_ransac_threshold; |
| 716 | CHECK(nh_private_.getParam("velocity_det_threshold", vel_det_threshold)); |
| 717 | CHECK(nh_private_.getParam("position_det_threshold", pos_det_threshold)); |
| 718 | CHECK( |
| 719 | nh_private_.getParam("stereo_ransac_threshold", stereo_ransac_threshold)); |
| 720 | CHECK(nh_private_.getParam("mono_ransac_threshold", mono_ransac_theshold)); |
| 721 | resiliency_msg.data[4] = pos_det_threshold; |
| 722 | resiliency_msg.data[5] = vel_det_threshold; |
| 723 | resiliency_msg.data[6] = stereo_ransac_threshold; |
| 724 | resiliency_msg.data[7] = mono_ransac_theshold; |
| 725 | |
| 726 | // Build Message Layout |
| 727 | resiliency_msg.layout.dim.resize(1); |
| 728 | resiliency_msg.layout.dim[0].size = resiliency_msg.data.size(); |
| 729 | resiliency_msg.layout.dim[0].stride = 1; |