better visualization and violation checking

Author: nubertj

Diff for: graph_msf/include/graph_msf/interface/GraphMsf.h

@@ -12,6 +12,7 @@ Please see the LICENSE file that has been included as part of this package.
 #include <mutex>
 #include <stdexcept>
 #include <thread>
+#include <unordered_set>
 
 // Package
 #include "graph_msf/config/GraphConfig.h"
@@ -39,8 +40,8 @@ class GraphMsf {
   void setup(const std::shared_ptr<GraphConfig> graphConfigPtr, const std::shared_ptr<StaticTransforms> staticTransformsPtr);
 
   // Initialization Interface
-  bool initYawAndPositionInWorld(const double yaw_W_frame1, const Eigen::Vector3d& W_t_W_frame2,
-                                 const std::string& frame1, const std::string& frame2);
+  bool initYawAndPositionInWorld(const double yaw_W_frame1, const Eigen::Vector3d& W_t_W_frame2, const std::string& frame1,
+                                 const std::string& frame2);
   bool initYawAndPosition(const UnaryMeasurementXD<Eigen::Isometry3d, 6>& unary6DMeasurement);
 
   // Trigger offline smoother optimization
@@ -104,6 +105,9 @@ class GraphMsf {
   // Initialization
   void pretendFirstMeasurementReceived();
 
+  // Check whether measurement violated covariance, if yes, add to set and print once, if not, remove from set and print that returned
+  bool checkAndPrintCovarianceViolation_(const std::string& measurementName, const bool violatedFlag);
+
   // Members
   // Graph Config
   std::shared_ptr<GraphConfig> graphConfigPtr_ = nullptr;
@@ -131,6 +135,9 @@ class GraphMsf {
   // Counter
   long imuCallbackCounter_ = 0;
 
+  // Set of measurements that have violated the covariance
+  std::unordered_set<std::string> measurementsWithViolatedCovariance_;
+
  private:
   /// Worker functions
   //// Set Imu Attitude

Diff for: graph_msf/src/lib/GraphMsf.cpp

@@ -388,4 +388,22 @@ void GraphMsf::pretendFirstMeasurementReceived() {
   validFirstMeasurementReceivedFlag_ = true;
 }
 
+// Check whether measurement violated covariance, if yes, add to set and print once, if not, remove from set and print that returned
+bool GraphMsf::checkAndPrintCovarianceViolation_(const std::string& measurementName, const bool violatedFlag) {
+  if (violatedFlag) {
+    if (measurementsWithViolatedCovariance_.find(measurementName) == measurementsWithViolatedCovariance_.end()) {
+      measurementsWithViolatedCovariance_.insert(measurementName);
+      REGULAR_COUT << RED_START << " " << measurementName << " covariance violated. Not adding factor until not violated anymore."
+                   << COLOR_END << std::endl;
+    }
+    return true;
+  } else {
+    if (measurementsWithViolatedCovariance_.find(measurementName) != measurementsWithViolatedCovariance_.end()) {
+      measurementsWithViolatedCovariance_.erase(measurementName);
+      REGULAR_COUT << GREEN_START << " " << measurementName << " covariance not violated anymore." << COLOR_END << std::endl;
+    }
+    return false;
+  }
+}
+
 }  // namespace graph_msf

Diff for: graph_msf/src/lib/GraphMsfHolistic.cpp

@@ -48,8 +48,7 @@ void GraphMsfHolistic::addUnaryPose3AbsoluteMeasurement(const UnaryMeasurementXD
   } else {  // Graph initialized
     // Check for covariance violation
     bool covarianceViolatedFlag = isCovarianceViolated_<6>(R_T_R_S.unaryMeasurementNoiseDensity(), R_T_R_S.covarianceViolationThreshold());
-    if (covarianceViolatedFlag) {
-      REGULAR_COUT << RED_START << " Pose covariance violated. Not adding factor." << COLOR_END << std::endl;
+    if (checkAndPrintCovarianceViolation_(R_T_R_S.measurementName(), covarianceViolatedFlag)) {
       return;
     }
 
@@ -86,9 +85,7 @@ void GraphMsfHolistic::addUnaryPosition3AbsoluteMeasurement(
     // Check for covariance violation
     bool covarianceViolatedFlag = isCovarianceViolated_<3>(fixedFrame_t_fixedFrame_sensorFrame.unaryMeasurementNoiseDensity(),
                                                            fixedFrame_t_fixedFrame_sensorFrame.covarianceViolationThreshold());
-    if (covarianceViolatedFlag) {
-      REGULAR_COUT << RED_START << " " << fixedFrame_t_fixedFrame_sensorFrame.measurementName()
-                   << " covariance violated. Not adding factor." << COLOR_END << std::endl;
+    if (checkAndPrintCovarianceViolation_(fixedFrame_t_fixedFrame_sensorFrame.measurementName(), covarianceViolatedFlag)) {
       return;
     }
 
@@ -141,8 +138,7 @@ void GraphMsfHolistic::addUnaryVelocity3LocalMeasurement(UnaryMeasurementXD<Eige
   } else {  // Case 2: Graph Initialized
     // Check for covariance violation
     bool covarianceViolatedFlag = isCovarianceViolated_<3>(S_v_F_S.unaryMeasurementNoiseDensity(), S_v_F_S.covarianceViolationThreshold());
-    if (covarianceViolatedFlag) {
-      REGULAR_COUT << RED_START << " Velocity covariance violated. Not adding factor." << COLOR_END << std::endl;
+    if (checkAndPrintCovarianceViolation_(S_v_F_S.measurementName(), covarianceViolatedFlag)) {
       return;
     }
 
@@ -178,8 +174,7 @@ void GraphMsfHolistic::addUnaryPosition3LandmarkMeasurement(UnaryMeasurementXDLa
   } else {  // Case 2: Graph Initialized
     // Check for covariance violation
     bool covarianceViolatedFlag = isCovarianceViolated_<3>(S_t_S_L.unaryMeasurementNoiseDensity(), S_t_S_L.covarianceViolationThreshold());
-    if (covarianceViolatedFlag) {
-      REGULAR_COUT << RED_START << " Position covariance violated. Not adding factor." << COLOR_END << std::endl;
+    if (checkAndPrintCovarianceViolation_(S_t_S_L.measurementName(), covarianceViolatedFlag)) {
       return;
     }
 

Diff for: ros/graph_msf_ros/include/graph_msf_ros/GraphMsfRos.h

@@ -69,17 +69,18 @@ class GraphMsfRos : public GraphMsfClassic, public GraphMsfHolistic {
       const geometry_msgs::PoseWithCovarianceStampedPtr& msgPtr, const std::string& frameName, const ros::Time& stamp,
       const Eigen::Isometry3d& T, const Eigen::Matrix<double, 6, 6>& transformCovariance = Eigen::Matrix<double, 6, 6>::Zero());
   static void addToLandmarkMarkerArrayMsg(const visualization_msgs::MarkerArrayPtr& markerArrayPtr, const std::string& referenceFrameName,
-                                  const ros::Time& stamp, const Eigen::Isometry3d& T_R_L, const std::string& transformName);
+                                          const ros::Time& stamp, const Eigen::Isometry3d& T_R_L, const std::string& transformName);
 
   // Extract from State
   static void extractCovariancesFromOptimizedState(
       Eigen::Matrix<double, 6, 6>& poseCovarianceRos, Eigen::Matrix<double, 6, 6>& twistCovarianceRos,
       const std::shared_ptr<graph_msf::SafeNavStateWithCovarianceAndBias>& optimizedStateWithCovarianceAndBiasPtr);
   // Markers
   static void createVelocityMarker(const std::string& referenceFrameName, const ros::Time& stamp, const Eigen::Vector3d& velocity,
-                                   visualization_msgs::Marker& marker);
+                                   const Eigen::Vector3d& colorRgb, visualization_msgs::Marker& marker);
   static void createContactMarker(const std::string& referenceFrameName, const ros::Time& stamp, const Eigen::Vector3d& position,
-                           const std::string& nameSpace, const int id, const Eigen::Vector3d& colorRgb, visualization_msgs::Marker& marker);
+                                  const std::string& nameSpace, const int id, const Eigen::Vector3d& colorRgb,
+                                  visualization_msgs::Marker& marker);
 
   // Callbacks
   virtual void imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr);

Diff for: ros/graph_msf_ros/src/lib/GraphMsfRos.cpp

@@ -248,7 +248,7 @@ void GraphMsfRos::extractCovariancesFromOptimizedState(
 
 // Markers
 void GraphMsfRos::createVelocityMarker(const std::string& referenceFrameName, const ros::Time& stamp, const Eigen::Vector3d& velocity,
-                                       visualization_msgs::Marker& marker) {
+                                       const Eigen::Vector3d& colorRgb, visualization_msgs::Marker& marker) {
   // Arrow
   marker.header.frame_id = referenceFrameName;
   marker.header.stamp = stamp;
@@ -261,9 +261,9 @@ void GraphMsfRos::createVelocityMarker(const std::string& referenceFrameName, co
   marker.scale.y = 0.2;  // head diameter
   marker.scale.z = 0.2;  // head length
   marker.color.a = 1.0;
-  marker.color.r = 1.0;
-  marker.color.g = 0.0;
-  marker.color.b = 0.0;
+  marker.color.r = colorRgb(0);
+  marker.color.g = colorRgb(1);
+  marker.color.b = colorRgb(2);
   // Define Arrow through start and end point
   geometry_msgs::Point startPoint, endPoint;
   startPoint = geometry_msgs::Point();
@@ -604,9 +604,12 @@ void GraphMsfRos::publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceR
 }
 
 void GraphMsfRos::publishVelocityMarkers(const std::shared_ptr<const graph_msf::SafeIntegratedNavState>& navStatePtr) const {
+  // Color
+  Eigen::Vector3d colorRgb(1.0, 0.0, 0.0);  // Red
   // Velocity in Odom Frame Marker
   visualization_msgs::Marker velocityMarker;
-  createVelocityMarker(staticTransformsPtr_->getImuFrame(), ros::Time(navStatePtr->getTimeK()), navStatePtr->getI_v_W_I(), velocityMarker);
+  createVelocityMarker(staticTransformsPtr_->getImuFrame(), ros::Time(navStatePtr->getTimeK()), navStatePtr->getI_v_W_I(), colorRgb,
+                       velocityMarker);
   // Publish
   if (pubVelocityMarker_.getNumSubscribers() > 0) {
     pubVelocityMarker_.publish(velocityMarker);