Perception: Handle velocity with NaN; Removed some warnings

modules/common/time/time_util.h

@@ -51,7 +51,8 @@ class TimeUtil {
   static double GetCurrentTime() {
     struct timeval tv;
     gettimeofday(&tv, nullptr);
-    const double timestamp = tv.tv_sec * 1000000 + tv.tv_usec;
+    const double timestamp =
+      static_cast<double>(tv.tv_sec * 1000000 + tv.tv_usec);
     return timestamp / 1000000;
   }
 

modules/perception/camera/lib/obstacle/tracker/omt/omt_obstacle_tracker.cc

@@ -133,9 +133,9 @@ bool OMTObstacleTracker::CombineDuplicateTargets() {
                 << "," << targets_[j].id
                 << ") score " << score
                 << " count " << count;
-      hypo.target = i;
-      hypo.object = j;
-      hypo.score = (count > 0) ? score / (count) : 0;
+      hypo.target = static_cast<int>(i);
+      hypo.object = static_cast<int>(j);
+      hypo.score = (count > 0) ? score / static_cast<float>(count) : 0;
       if (hypo.score < omt_param_.target_combine_iou_threshold()) {
         continue;
       }
@@ -185,8 +185,8 @@ void OMTObstacleTracker::GenerateHypothesis(const TrackObjectPtrs &objects) {
   for (size_t i = 0; i < targets_.size(); ++i) {
     ADEBUG << "Target " << targets_[i].id;
     for (size_t j = 0; j < objects.size(); ++j) {
-      hypo.target = i;
-      hypo.object = j;
+      hypo.target = static_cast<int>(i);
+      hypo.object = static_cast<int>(j);
       float sa = ScoreAppearance(targets_[i], objects[j]);
       float sm = ScoreMotion(targets_[i], objects[j]);
       float ss = ScoreShape(targets_[i], objects[j]);
@@ -279,7 +279,7 @@ float OMTObstacleTracker::ScoreAppearance(const Target &target,
     count += 1;
   }
 
-  return energy / (0.1f + count * 0.9f);
+  return energy / (0.1f + static_cast<float>(count) * 0.9f);
 }
 
 // [new]
@@ -347,8 +347,8 @@ int OMTObstacleTracker::CreateNewTarget(const TrackObjectPtrs &objects) {
         continue;
       }
       for (auto &&target_rect : target_rects) {
-        if (IsCovered(rect, target_rect, 0.4) ||
-            IsCoveredHorizon(rect, target_rect, 0.5)) {
+        if (IsCovered(rect, target_rect, 0.4f) ||
+            IsCoveredHorizon(rect, target_rect, 0.5f)) {
           is_covered = true;
           break;
         }
@@ -391,9 +391,9 @@ bool OMTObstacleTracker::Associate2D(const ObstacleTrackerOptions &options,
     // TODO(gaohan): use pool
     TrackObjectPtr track_ptr(new TrackObject);
     track_ptr->object = frame->detected_objects[i];
-    track_ptr->object->id = i;
+    track_ptr->object->id = static_cast<int>(i);
     track_ptr->timestamp = frame->timestamp;
-    track_ptr->indicator = PatchIndicator(frame->frame_id, i,
+    track_ptr->indicator = PatchIndicator(frame->frame_id, static_cast<int>(i),
                                           frame->data_provider->sensor_name());
     track_ptr->object->camera_supplement.sensor_name =
         frame->data_provider->sensor_name();

modules/perception/fusion/base/sensor.cc

@@ -55,7 +55,7 @@ bool Sensor::GetPose(double timestamp, Eigen::Affine3d* pose) const {
 
   for (int i = static_cast<int>(frames_.size()) - 1; i >= 0; --i) {
     double time_diff = timestamp - frames_[i]->GetTimestamp();
-    if (fabs(time_diff) < 1.0e-3) {
+    if (fabs(time_diff) < 1.0e-3) {  // > ?
       return frames_[i]->GetPose(pose);
     }
   }

modules/perception/fusion/common/kalman_filter.cc

@@ -28,7 +28,7 @@ bool KalmanFilter::Init(const Eigen::VectorXd &initial_belief_states,
     AERROR << "the cols and rows of uncertainty martix should be equal";
     return false;
   }
-  states_num_ = initial_uncertainty.rows();
+  states_num_ = static_cast<int>(initial_uncertainty.rows());
 
   if (states_num_ <= 0) {
     AERROR << "state_num should be greater than zero";

modules/perception/fusion/common/kalman_filter.h

@@ -55,9 +55,9 @@ class KalmanFilter : public BaseFilter {
   bool DeCorrelation(int x, int y, int x_len, int y_len);
   void CorrectionBreakdown();
   bool SetGainBreakdownThresh(const std::vector<bool> &break_down,
-                              const float threshold = 2.0);
+                              const float threshold = 2.0f);
   bool SetValueBreakdownThresh(const std::vector<bool> &break_down,
-                               const float threshold = 0.05);
+                               const float threshold = 0.05f);
 
  private:
   // @brief kalman gain
@@ -66,8 +66,8 @@ class KalmanFilter : public BaseFilter {
   Eigen::VectorXd value_break_down_;
   Eigen::MatrixXd kalman_gain_;
 
-  float value_break_down_threshold_ = 999.0;
-  float gain_break_down_threshold_ = 0.0;
+  float value_break_down_threshold_ = 999.0f;
+  float gain_break_down_threshold_ = 0.0f;
 };
 
 }  // namespace fusion

modules/perception/fusion/lib/data_fusion/motion_fusion/kalman_motion_fusion/kalman_motion_fusion.cc

@@ -372,8 +372,8 @@ void KalmanMotionFusion::RewardRMatrix(const base::SensorType& sensor_type,
                                        Eigen::MatrixXd* r_matrix) {
   common::SensorManager* sensor_manager =
       lib::Singleton<common::SensorManager>::get_instance();
-  const float converged_scale = 0.01;
-  const float unconverged_scale = 1000.0;
+  const float converged_scale = 0.01f;
+  const float unconverged_scale = 1000.0f;
   if (sensor_manager->IsLidar(sensor_type)) {
     if (converged) {
       r_matrix->setIdentity();

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_filter.cc

@@ -148,6 +148,9 @@ void MlfMotionFilter::KalmanFilterUpdateWithPartialObservation(
 
   double time_diff = new_object->object_ptr->latest_tracked_time -
                      latest_object->object_ptr->latest_tracked_time;
+  if (time_diff < EPSION_TIME) {  // Very small time than assign
+    time_diff = DEFAULT_FPS;
+  }
   Eigen::Matrix4d transition = Eigen::Matrix4d::Identity();
   transition(0, 2) = transition(1, 3) = time_diff;
 

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_filter.h

@@ -135,6 +135,8 @@ class MlfMotionFilter : public MlfBaseFilter {
   void BeliefToOutput(TrackedObjectPtr object);
 
  protected:
+  const double EPSION_TIME = 1e-3;
+  const double DEFAULT_FPS = 0.1;
   // motion measurement
   std::shared_ptr<MlfMotionMeasurement> motion_measurer_;
   // motion refiner

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_measurement.cc

@@ -41,7 +41,9 @@ void MlfMotionMeasurement::ComputeMotionMeasurment(
   double latest_time = latest_object->object_ptr->latest_tracked_time;
   double current_time = new_object->object_ptr->latest_tracked_time;
   double time_diff = current_time - latest_time;
-
+  if (fabs(time_diff) < EPSILON_TIME) {
+    time_diff = DEFAULT_FPS;
+  }
   MeasureAnchorPointVelocity(new_object, latest_object, time_diff);
   MeasureBboxCenterVelocity(new_object, latest_object, time_diff);
   MeasureBboxCornerVelocity(new_object, latest_object, time_diff);

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_measurement.h

@@ -51,6 +51,8 @@ class MlfMotionMeasurement {
       TrackedObjectPtr new_object);
 
  private:
+  const double EPSILON_TIME = 1e-3;  // or numeric_limits<double>::epsilon()
+  const double DEFAULT_FPS = 0.1;
   DISALLOW_COPY_AND_ASSIGN(MlfMotionMeasurement);
 };  // class MlfMotionMeasurement
 

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_refiner.cc

@@ -56,7 +56,12 @@ bool MlfMotionRefiner::Refine(const MlfTrackDataConstPtr& track_data,
   double time_diff = new_object->object_ptr->latest_tracked_time -
                      latest_object->object_ptr->latest_tracked_time;
   Eigen::Vector3d filter_velocity_gain = new_object->belief_velocity_gain;
-  double filter_acceleration_gain = filter_velocity_gain.norm() / time_diff;
+  double filter_acceleration_gain = 0.0;
+  if (fabs(time_diff) < EPSION_TIME) {
+    filter_acceleration_gain = 0.0;
+  } else {
+    filter_acceleration_gain = filter_velocity_gain.norm() / time_diff;
+  }
   bool need_keep_motion =
       filter_acceleration_gain > claping_acceleration_threshold_;
   // use tighter threshold for pedestrian

modules/perception/lidar/lib/tracker/multi_lidar_fusion/mlf_motion_refiner.h

@@ -70,6 +70,7 @@ class MlfMotionRefiner {
  protected:
   double claping_acceleration_threshold_ = 10;
   double claping_speed_threshold_ = 1.0;
+  const double EPSION_TIME = 1e-3;
 };  // class MlfMotionRefiner
 
 }  // namespace lidar

modules/perception/onboard/component/fusion_camera_detection_component.cc

@@ -331,9 +331,10 @@ int FusionCameraDetectionComponent::InitConfig() {
       fusion_camera_detection_param.output_final_obstacles();
   prefused_channel_name_ =
       fusion_camera_detection_param.prefused_channel_name();
-  default_camera_pitch_ = fusion_camera_detection_param.default_camera_pitch();
+  default_camera_pitch_ =
+    static_cast<float>(fusion_camera_detection_param.default_camera_pitch());
   default_camera_height_ =
-      fusion_camera_detection_param.default_camera_height();
+    static_cast<float>(fusion_camera_detection_param.default_camera_height());
   output_camera_debug_msg_ =
       fusion_camera_detection_param.output_camera_debug_msg();
   camera_debug_channel_name_ =
@@ -398,8 +399,8 @@ int FusionCameraDetectionComponent::InitSensorInfo() {
   // assume all camera have same image size
   base::BaseCameraModelPtr camera_model_ptr =
       sensor_manager->GetUndistortCameraModel(camera_names_[0]);
-  image_width_ = camera_model_ptr->get_width();
-  image_height_ = camera_model_ptr->get_height();
+  image_width_ = static_cast<int>(camera_model_ptr->get_width());
+  image_height_ = static_cast<int>(camera_model_ptr->get_height());
 
   std::string format_str = R"(
       camera_names: %s %s
@@ -704,7 +705,7 @@ int FusionCameraDetectionComponent::ConvertObjectToPb(
   pb_msg->set_height(object_ptr->size(2));
 
   // convert 3d bbox to polygon
-  int polygon_point_size = object_ptr->polygon.size();
+  int polygon_point_size = static_cast<int>(object_ptr->polygon.size());
   for (int i = 0; i < polygon_point_size; ++i) {
     auto &pt = object_ptr->polygon.at(i);
     apollo::common::Point3D *p = pb_msg->add_polygon_point();