Validate odometry and fixed frame poses. (#588)

cartographer/mapping/global_trajectory_builder.h

@@ -19,6 +19,8 @@
 
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
 
+#include "glog/logging.h"
+
 namespace cartographer {
 namespace mapping {
 
@@ -61,12 +63,14 @@ class GlobalTrajectoryBuilder
   }
 
   void AddSensorData(const sensor::OdometryData& odometry_data) override {
+    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
     local_trajectory_builder_.AddOdometerData(odometry_data);
     sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
   }
 
   void AddSensorData(
       const sensor::FixedFramePoseData& fixed_frame_pose) override {
+    CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose;
     sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
   }
 

cartographer/mapping/id.h

@@ -236,9 +236,7 @@ class MapById {
         typename std::map<int, MapByIndex>::const_iterator current_trajectory)
         : current_trajectory_(current_trajectory) {}
 
-    int operator*() const {
+    int operator*() const { return current_trajectory_->first; }
-      return current_trajectory_->first;
-    }
 
     ConstTrajectoryIterator& operator++() {
       ++current_trajectory_;
@@ -254,13 +252,14 @@ class MapById {
       return current_trajectory_ == it.current_trajectory_;
     }
 
-    bool operator!=(const ConstTrajectoryIterator& it) const { return !operator==(it); }
+    bool operator!=(const ConstTrajectoryIterator& it) const {
+      return !operator==(it);
+    }
 
    private:
     typename std::map<int, MapByIndex>::const_iterator current_trajectory_;
   };
 
-
   // Appends data to a 'trajectory_id', creating trajectories as needed.
   IdType Append(const int trajectory_id, const DataType& data) {
     CHECK_GE(trajectory_id, 0);

cartographer/transform/rigid_transform.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_
 #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_
 
+#include <cmath>
 #include <iostream>
 #include <string>
 
@@ -180,6 +181,12 @@ class Rigid3 {
     return out;
   }
 
+  bool IsValid() const {
+    return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) &&
+           !std::isnan(translation_.z()) &&
+           std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3);
+  }
+
  private:
   Vector translation_;
   Quaternion rotation_;