perception: finish debugging offline_sequential_obstacle_perception_test

modules/perception/obstacle/common/file_system_util.cc

@@ -36,7 +36,7 @@ std::string GetFileName(const std::string& path) {
 
 void GetFileNamesInFolderById(const std::string& folder, const std::string& ext,
                               std::vector<std::string>* ret) {
-  std::vector<int> ret_id;
+  std::vector<double> ret_id;
   ret->clear();
   namespace fs = boost::filesystem;
   if (!fs::exists(folder) || !fs::is_directory(folder)) {
@@ -53,7 +53,7 @@ void GetFileNamesInFolderById(const std::string& folder, const std::string& ext,
       std::string temp_id_str =
           temp_path.substr(temp_path.rfind('_') + 1,
                            temp_path.rfind('.') - temp_path.rfind('_') - 1);
-      int temp_id = std::atoi(temp_id_str.c_str());
+      double temp_id = std::atof(temp_id_str.c_str());
       ret_id.push_back(temp_id);
     }
     ++it;
@@ -63,7 +63,7 @@ void GetFileNamesInFolderById(const std::string& folder, const std::string& ext,
   for (int i = 0; i < ret_size; ++i) {
     for (int j = i; j < ret_size; ++j) {
       if (ret_id[i] > ret_id[j]) {
-        int temp_id = ret_id[i];
+        double temp_id = ret_id[i];
         ret_id[i] = ret_id[j];
         ret_id[j] = temp_id;
         std::string temp_path = (*ret)[i];

modules/perception/obstacle/common/pose_util.cc

@@ -24,6 +24,7 @@ namespace perception {
 
 bool ReadPoseFile(const std::string &filename, Eigen::Matrix4d *pose,
                   int *frame_id, double *time_stamp) {
+  *pose = Eigen::Matrix4d::Identity();
   std::ifstream ifs(filename.c_str());
   if (!ifs.is_open()) {
     AERROR << "Failed to open file " << filename;
@@ -58,6 +59,23 @@ bool ReadPoseFile(const std::string &filename, Eigen::Matrix4d *pose,
   return true;
 }
 
+bool ReadPoseFileMat12(const std::string &filename, Eigen::Matrix4d *pose,
+                       int *frame_id, double *time_stamp) {
+  *pose = Eigen::Matrix4d::Identity();
+  std::ifstream ifs(filename.c_str());
+  if (!ifs.is_open()) {
+    AERROR << "Failed to open file " << filename;
+    return false;
+  }
+  ifs >> *frame_id >> *time_stamp;
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 4; j++) {
+      ifs >> (*pose)(i, j);
+    }
+  }
+  return true;
+}
+
 bool LoadExtrinsic(const std::string &file_path, Eigen::Affine3d *extrinsic) {
   try {
     YAML::Node config = YAML::LoadFile(file_path);

modules/perception/obstacle/common/pose_util.h

@@ -54,6 +54,9 @@ void QuaternionToRotationMatrix(const T* quat, T* R) {
 bool ReadPoseFile(const std::string& filename, Eigen::Matrix4d* pose,
                   int* frame_id, double* time_stamp);
 
+bool ReadPoseFileMat12(const std::string &filename, Eigen::Matrix4d *pose,
+                  int *frame_id, double *time_stamp);
+
 // @brief: Load the velodyne extrinsic from a YAML file.
 // @param [in]: path to yaml file
 // @param [out]: extrinsic transform

modules/perception/obstacle/fusion/dummy/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "perception_obstacle_fusion_dummy",
+    srcs = [
+        "dummy_algorithms.cc",
+    ],
+    hdrs = [
+        "dummy_algorithms.h",
+    ],
+    deps = [
+        "//modules/common:log",
+        "//modules/perception/obstacle/common:perception_obstacle_common",
+        "//modules/perception/obstacle/fusion/interface:perception_obstacle_fusion_interface",
+        "@eigen//:eigen",
+        "@pcl//:pcl",
+    ],
+)
+
+cpplint()

modules/perception/obstacle/fusion/dummy/dummy_algorithms.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/perception/obstacle/fusion/dummy/dummy_algorithms.h"
+
+namespace apollo {
+namespace perception {
+
+bool DummyFusion::Fuse(const std::vector<SensorObjects> &multi_sensor_objects,
+                    std::vector<ObjectPtr> *fused_objects) {
+  return result_detect_;
+}
+
+}  // namespace perception
+}  // namespace apollo

modules/perception/obstacle/fusion/dummy/dummy_algorithms.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#ifndef MODULES_PERCEPTION_OBSTACLE_FUSION_DUMMY_DUMMY_ALGORITHMS_H_
+#define MODULES_PERCEPTION_OBSTACLE_FUSION_DUMMY_DUMMY_ALGORITHMS_H_
+
+#include <string>
+#include <vector>
+
+#include "modules/perception/obstacle/fusion/interface/base_fusion.h"
+
+namespace apollo {
+namespace perception {
+
+class DummyFusion : public BaseFusion {
+ public:
+  DummyFusion() : BaseFusion() {}
+  ~DummyFusion() = default;
+
+  bool Init() override {
+    return result_init_;
+  }
+
+  bool Fuse(const std::vector<SensorObjects> &multi_sensor_objects,
+                    std::vector<ObjectPtr> *fused_objects) override;
+
+  std::string name() const override {
+    return "DummyFusion";
+  }
+
+ private:
+  // for unit test
+  bool result_init_ = true;
+  bool result_detect_ = true;
+
+  DISALLOW_COPY_AND_ASSIGN(DummyFusion);
+};
+
+REGISTER_FUSION(DummyFusion);
+}  // namespace perception
+}  // namespace apollo
+
+#endif  // MODULES_PERCEPTION_OBSTACLE_FUSION_DUMMY_DUMMY_ALGORITHMS_H_

modules/perception/obstacle/lidar/visualizer/opengl_visualizer/frame_content.cc

@@ -129,8 +129,18 @@ void FrameContent::SetTrackedObjectsFused(
   }
 }
 
+void FrameContent::SetTrackedObjects(
+    const std::vector<ObjectPtr> &objects) {
+  tracked_objects_.resize(objects.size());
+  for (size_t i = 0; i < objects.size(); ++i) {
+    tracked_objects_[i].reset(new Object);
+    tracked_objects_[i]->clone(*objects[i]);
+    OffsetObject(tracked_objects_[i], global_offset_);
+  }
+}
+
 std::vector<ObjectPtr> FrameContent::GetTrackedObjects() {
-  return tracked_objects_lidar_;
+  return tracked_objects_;
 }
 
 }  // namespace perception

modules/perception/obstacle/lidar/visualizer/opengl_visualizer/frame_content.h

@@ -48,6 +48,7 @@ class FrameContent {
   void SetTrackedObjectsLidar(const std::vector<ObjectPtr> &objects);
   void SetTrackedObjectsRadar(const std::vector<ObjectPtr> &objects);
   void SetTrackedObjectsFused(const std::vector<ObjectPtr> &objects);
+  void SetTrackedObjects(const std::vector<ObjectPtr> &objects);
   std::vector<ObjectPtr> GetTrackedObjects();
 
  protected:
@@ -70,6 +71,7 @@ class FrameContent {
   std::vector<ObjectPtr> tracked_objects_lidar_;  // after tracking
   std::vector<ObjectPtr> tracked_objects_radar_;  // after tracking
   std::vector<ObjectPtr> tracked_objects_fused_;  // after tracking
+  std::vector<ObjectPtr> tracked_objects_;  // after tracking
 };
 
 }  // namespace perception

modules/perception/obstacle/onboard/BUILD

@@ -42,6 +42,8 @@ cc_library(
         "//modules/perception/obstacle/lidar/segmentation/cnnseg:perception_obstacle_lidar_segmention_cnnseg",
         "//modules/perception/obstacle/lidar/tracker/hm_tracker:perception_obstacle_lidar_tracker_hm_tracker",
         "//modules/perception/obstacle/lidar/visualizer/opengl_visualizer:perception_obstacle_lidar_opengl_visualizer",
+        "//modules/perception/obstacle/radar/dummy:perception_obstacle_radar_dummy",
+        "//modules/perception/obstacle/fusion/dummy:perception_obstacle_fusion_dummy",
         "@eigen//:eigen",
         "@gtest//:gtest",
         "@ros//:ros_common",

modules/perception/obstacle/onboard/lidar_process.cc

@@ -224,6 +224,9 @@ bool LidarProcess::InitFrameDependence() {
     AINFO << "get and Init hdmap_input succ.";
   }
 
+  /// init roi indices
+  roi_indices_ = pcl_util::PointIndicesPtr(new pcl_util::PointIndices);
+
   return true;
 }
 

modules/perception/obstacle/onboard/obstacle_perception.cc

@@ -19,6 +19,14 @@
 #include "modules/common/time/time.h"
 #include "modules/perception/common/perception_gflags.h"
 #include "modules/perception/lib/base/timer.h"
+#include "modules/perception/obstacle/radar/dummy/dummy_algorithms.h"
+#include "modules/perception/obstacle/radar/modest/modest_radar_detector.h"
+#include "modules/perception/obstacle/fusion/dummy/dummy_algorithms.h"
+#include "modules/perception/obstacle/fusion/probabilistic_fusion/probabilistic_fusion.h"
+
+DEFINE_bool(show_lidar_obstacles, false, "whether show lidar obstacles or not");
+DEFINE_bool(show_radar_obstacles, false, "whether show radar obstacles or not");
+DEFINE_bool(show_fused_obstacles, false, "whether show fused obstacles or not");
 
 namespace apollo {
 namespace perception {
@@ -28,6 +36,7 @@ ObstaclePerception::ObstaclePerception() : initialized_(false) {}
 ObstaclePerception::~ObstaclePerception() {}
 
 bool ObstaclePerception::Init() {
+  RegistAllAlgorithm();
   // initialize lidar detector
   lidar_perception_.reset(new LidarProcess);
   if (lidar_perception_ == nullptr) {
@@ -81,59 +90,70 @@ void ObstaclePerception::SetGlobalOffset(const Eigen::Vector3d& global_offset) {
   global_offset_ = global_offset;
 }
 
+void ObstaclePerception::RegistAllAlgorithm() {
+  RegisterFactoryDummyRadarDetector();
+  RegisterFactoryDummyFusion();
+
+  RegisterFactoryModestRadarDetector();
+  RegisterFactoryProbabilisticFusion();
+}
+
 bool ObstaclePerception::Process(SensorRawFrame* frame,
                                  std::vector<ObjectPtr>* out_objects) {
-  if (frame == nullptr) {
+  if (frame == nullptr || out_objects == nullptr) {
     return false;
   }
   PERF_BLOCK_START();
 
   std::shared_ptr<SensorObjects> sensor_objects(new SensorObjects());
   if (frame->sensor_type_ == VELODYNE_64) {
+    // lidar obstacle detection
     VelodyneRawFrame* velodyne_frame = dynamic_cast<VelodyneRawFrame*>(frame);
-    if (lidar_perception_->Process(
-            velodyne_frame->timestamp_, velodyne_frame->cloud_,
-            std::make_shared<Eigen::Matrix4d>(velodyne_frame->pose_))) {
-      sensor_objects->objects = lidar_perception_->GetObjects();
-      frame_content_.SetLidarPose(velodyne_frame->pose_);
-      frame_content_.SetLidarCloud(velodyne_frame->cloud_);
-      // _frame_content.SetLidarRoiCloud(roi_cloud);
-      frame_content_.SetTrackedObjectsLidar(sensor_objects->objects);
-    } else {
+    std::shared_ptr<Eigen::Matrix4d> velodyne_pose(new Eigen::Matrix4d);
+    *(velodyne_pose.get()) = velodyne_frame->pose_;
+    if (!lidar_perception_->Process(velodyne_frame->timestamp_,
+                                    velodyne_frame->cloud_, velodyne_pose)) {
       AERROR << "Lidar perception error!, " << std::fixed
              << std::setprecision(12) << velodyne_frame->timestamp_;
       return false;
     }
-    // _frame_content.update_timestamp(velodyne_frame->_timestamp);
+    sensor_objects->objects = lidar_perception_->GetObjects();
     PERF_BLOCK_END("lidar_perception");
+    // set frame content
+    if (FLAGS_enable_visualization) {
+      frame_content_.SetLidarPose(velodyne_frame->pose_);
+      frame_content_.SetLidarCloud(velodyne_frame->cloud_);
+      if (FLAGS_show_lidar_obstacles) {
+        frame_content_.SetTrackedObjects(sensor_objects->objects);
+      }
+    }
   } else if (frame->sensor_type_ == RADAR) {
+    // radar obstacle detection
     RadarRawFrame* radar_frame = dynamic_cast<RadarRawFrame*>(frame);
     RadarDetectorOptions options;
     options.radar2world_pose = &(radar_frame->pose_);
     options.car_linear_speed = radar_frame->car_linear_speed_;
     std::vector<ObjectPtr> objects;
     std::vector<PolygonDType> map_polygons;
-    if (radar_detector_->Detect(radar_frame->raw_obstacles_, map_polygons,
+    if (!radar_detector_->Detect(radar_frame->raw_obstacles_, map_polygons,
                                 options, &objects)) {
-      if (!objects.empty()) {
-        sensor_objects->objects = objects;
-        frame_content_.SetTrackedObjectsRadar(objects);
-      } else {
-        AERROR << "Radar detector result is nullptr!";
-        return false;
-      }
-    } else {
       AERROR << "Radar perception error!, " << std::fixed
              << std::setprecision(12) << radar_frame->timestamp_;
       return false;
     }
+    sensor_objects->objects = objects;
+    AINFO << "radar objects size: " << objects.size();
     PERF_BLOCK_END("radar_detection");
+    // set frame content
+    if (FLAGS_enable_visualization && FLAGS_show_radar_obstacles) {
+      frame_content_.SetTrackedObjects(sensor_objects->objects);
+    }
   } else {
     AERROR << "Unknown sensor type : " << frame->sensor_type_;
     return false;
   }
   sensor_objects->sensor_type = frame->sensor_type_;
-  sensor_objects->sensor_id = GetSensorType(RADAR);
+  sensor_objects->sensor_id = GetSensorType(frame->sensor_type_);
   sensor_objects->timestamp = frame->timestamp_;
   sensor_objects->sensor2world_pose = frame->pose_;
 
@@ -147,15 +167,19 @@ bool ObstaclePerception::Process(SensorRawFrame* frame,
   }
   PERF_BLOCK_END("sensor_fusion");
 
-  *out_objects = fused_objects;
-  if (frame->sensor_type_ == VELODYNE_64) {
-    frame_content_.SetTrackedObjectsFused(fused_objects);
-  }
-
-  if (FLAGS_enable_visualization && initialized_) {
+  // // set frame content
+  if (FLAGS_enable_visualization) {
+    if (FLAGS_show_fused_obstacles) {
+      frame_content_.SetTrackedObjects(fused_objects);
+      if (frame->sensor_type_ != VELODYNE_64) {
+        return true;
+      }
+    }
+    frame_visualizer_->UpdateCameraSystem(&frame_content_);
     frame_visualizer_->Render(frame_content_);
   }
 
+  *out_objects = fused_objects;
   return true;
 }
 

modules/perception/obstacle/onboard/obstacle_perception.h

@@ -44,6 +44,7 @@ class ObstaclePerception {
   void SetGlobalOffset(const Eigen::Vector3d& global_offset);
 
  private:
+  void RegistAllAlgorithm();
   std::unique_ptr<LidarProcess> lidar_perception_;
   std::unique_ptr<BaseRadarDetector> radar_detector_;
   std::unique_ptr<BaseFusion> fusion_;

modules/perception/tool/export_sensor_data/BUILD

@@ -29,6 +29,7 @@ cc_binary(
     srcs = ["export_sensor_data_main.cc"],
     data = [
         "//modules/perception/conf:perception_adapter_manager_config",
+        "//modules/perception/tool/export_sensor_data/conf:perception_tool_export_sensor_data_config",
     ],
     deps = [
         ":export_sensor_data_lib",

modules/perception/tool/export_sensor_data/conf/BUILD

+package(default_visibility = ["//visibility:public"])
+
+filegroup(
+    name = "perception_tool_export_sensor_data_config",
+    srcs = [
+        "export_sensor_data.flag",
+    ],
+)

modules/perception/tool/export_sensor_data/conf/export_sensor_data.flag

 
+--work_root=modules/perception
+--config_manager_path=conf/config_manager.config
+
 ####################################################################
-# Flags from lib/config_manager/config_manager.cc
-# The ModelConfig config paths file.
+# Flags from obstacle/lidar/test/offline_lidar_perception_test.cpp
+# pcd path
 # type: string
-# default: ./conf/config_manager.config
---config_manager_path=./conf/config_manager.config
+# default: /apollo/data/pcd/
+--pcd_path=/apollo/data/pcd/
 
-# Project work root directory.
+# pose path
 # type: string
-# default: ""
---work_root=modules/perception
+# default: /apollo/data/pose/
+--pose_path=/apollo/data/pose/
+
+# output path
+# type: string
+# default: /apollo/data/output/
+--save_obstacles=false
+--output_path=/apollo/data/output/
+
+# enable visualization
+# type: bool
+# default: true
+--enable_visualization=true
 
 ####################################################################
-# Flags from obstacle/base/object.cc
-# Is serialize and output object cloud.
+# Flags from obstacle/onboard/hdmap_input.cc
+
+# roi distance of car center
+# type: double
+# default: 60.0
+--map_radius=60.0
+
+# step for sample road boundary points
+# type: int32
+# default: 1
+--map_sample_step=1
+
+--flagfile=modules/common/data/global_flagfile.txt
+
+####################################################################
+# Flags from obstacle/onboard/lidar_process.cc
+# enable hdmap input for roi filter
 # type: bool
 # default: false
---is_serialize_point_cloud=false
+--enable_hdmap_input=true
 
---flagfile=modules/common/data/global_flagfile.txt
+# roi filter before GroundDetector.
+# type: string
+# candidate: DummyROIFilter, HdmapROIFilter
+--onboard_roi_filter=HdmapROIFilter
+
+# the segmentation algorithm for onboard
+# type: string
+# candidate: DummySegmentation, CNNSegmentation
+--onboard_segmentor=CNNSegmentation
+
+# the object build algorithm for onboard
+# type: string
+# candidate: DummyObjectBuilder, MinBoxObjectBuilder
+--onboard_object_builder=MinBoxObjectBuilder
+
+# the tracking algorithm for onboard
+# type: string
+# candidate: DummyTracker, HmObjectTracker
+--onboard_tracker=HmObjectTracker
+
+--onboard_radar_detector=ModestRadarDetector
+
+# the perception module's output topic name.
+# type: string
+# default: perception_obstacle
+--obstacle_module_name=perception_obstacle
 
 # Query Ros TF timeout in ms. ros::Duration time.
 # type: int
 # default: 10
---tf2_buff_in_ms=20
+--tf2_buff_in_ms=10
 
 # ros TF2 quary frame id. tf2_buffer.lookupTransform.
 # type: string
@@ -35,17 +89,4 @@
 # default: velodyne64
 --lidar_tf2_child_frame_id=velodyne64
 
-# gps buffer size
-# type: int
-# default: 40
---gps_buffer_size=40
-
-# radar tf2 frame_id
-# type: string
-# default: radar_front
---radar_tf2_frame_id=world
-
-# radar tf2 child frame id
-# type: string
-# default: world
---radar_tf2_child_frame_id=radar_front
+--v=4

modules/perception/tool/export_sensor_data/export_sensor_data.cc

@@ -21,6 +21,7 @@
 #include <iostream>
 #include <fstream>
 
+#include "Eigen/Core"
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/log.h"
 #include "modules/perception/common/perception_gflags.h"
@@ -31,7 +32,7 @@
 #include "modules/perception/onboard/transform_input.h"
 
 DEFINE_string(lidar_path,
-  "modules/perception/tool/export_sensor_data/pcd/", "lidar path");
+  "modules/perception/tool/export_sensor_data/lidar/", "lidar path");
 DEFINE_string(radar_path,
   "modules/perception/tool/export_sensor_data/radar/", "radar path");
 
@@ -78,27 +79,26 @@ void ExportSensorData::WritePose(const std::string &file_pre,
   std::string filename = file_pre + ".pose";
   std::fstream fout(filename.c_str(), std::ios::out | std::ios::binary);
   if (!fout.is_open()) {
-    AINFO << "Failed to write radar pose.";
+    AINFO << "Failed to write pose.";
   }
-  fout << timestamp << " " << seq_num << " "
-       << pose(0, 0) << " " << pose(0, 1) << " "
-       << pose(0, 2) << " " << pose(0, 3) << " "
-       << pose(1, 0) << " " << pose(1, 1) << " "
-       << pose(1, 2) << " " << pose(1, 3) << " "
-       << pose(2, 0) << " " << pose(2, 1) << " "
-       << pose(2, 2) << " " << pose(2, 3);
+  Eigen::Matrix3f mat3f = pose.block<3, 3>(0, 0).cast<float>();
+  Eigen::Quaternionf quaternion(mat3f);
+  fout << std::setprecision(16) << seq_num << " " << timestamp << " "
+       << pose(0, 3) << " " << pose(1, 3) << " " << pose(2, 3) << " "
+       << quaternion.x() << " " << quaternion.y() << " "
+       << quaternion.z() << " " << quaternion.w() << std::endl;
   fout.close();
 }
 
-void ExportSensorData::WriteGpsInfo(const std::string &file_pre,
+void ExportSensorData::WriteVelocityInfo(const std::string &file_pre,
   const double& timestamp, const int seq_num,
   const Eigen::Vector3f& velocity) {
-  std::string filename = file_pre + ".gps";
+  std::string filename = file_pre + ".velocity";
   std::fstream fout(filename.c_str(), std::ios::out | std::ios::binary);
   if (!fout.is_open()) {
-    AINFO << "Failed to write gps.";
+    AINFO << "Failed to write velocity.";
   }
-  fout << timestamp << " " << seq_num << " "
+  fout << std::setprecision(16) << seq_num << " " << timestamp << " "
        << velocity(0) << " " << velocity(1) << " " << velocity(2);
   fout.close();
 }
@@ -156,7 +156,7 @@ void ExportSensorData::OnPointCloud(
   std::shared_ptr<Eigen::Matrix4d> velodyne_trans =
     std::make_shared<Eigen::Matrix4d>();
   if (!GetVelodyneTrans(kTimeStamp, velodyne_trans.get())) {
-    AERROR << "failed to get trans at timestamp: "
+    AERROR << "failed to get velodyne trans at timestamp: "
            << GLOG_TIMESTAMP(kTimeStamp);
     return;
   }
@@ -200,7 +200,8 @@ void ExportSensorData::OnRadar(const ContiRadar &radar_obs) {
   std::shared_ptr<Eigen::Matrix4d> radar2world_pose =
     std::make_shared<Eigen::Matrix4d>();
   if (!GetRadarTrans(timestamp, radar2world_pose.get())) {
-    AERROR << "Failed to get trans at timestamp: " << GLOG_TIMESTAMP(timestamp);
+    AERROR << "Failed to get radar trans at timestamp: "
+           << GLOG_TIMESTAMP(timestamp);
     return;
   }
   AINFO << "get radar trans pose succ. pose: \n" << *radar2world_pose;
@@ -219,7 +220,7 @@ void ExportSensorData::OnRadar(const ContiRadar &radar_obs) {
   AINFO << "radar file pre: " << file_pre;
   WriteRadar(file_pre, radar_obs_proto);
   WritePose(file_pre, timestamp, seq_num, *radar2world_pose);
-  WriteGpsInfo(file_pre, timestamp, seq_num, car_linear_speed);
+  WriteVelocityInfo(file_pre, timestamp, seq_num, car_linear_speed);
 }
 
 void ExportSensorData::OnGps(const apollo::localization::Gps &gps) {

modules/perception/tool/export_sensor_data/export_sensor_data.h

@@ -54,7 +54,7 @@ class ExportSensorData{
   void WritePose(const std::string &file_pre,
     const double timestamp, const int seq_num,
     const Eigen::Matrix4d& pose);
-  void WriteGpsInfo(const std::string &file_pre,
+  void WriteVelocityInfo(const std::string &file_pre,
     const double& timestamp, const int seq_num,
     const Eigen::Vector3f& velocity);
   void WritePCD(const std::string &file_pre,

modules/perception/tool/export_sensor_data/export_sensor_data_main.cc

@@ -27,8 +27,8 @@ int main(int argc, char* argv[]) {
   ros::AsyncSpinner spinner(4);
   AINFO << "Start export_sensor_data.";
   FLAGS_flagfile =
-      "./modules/perception/tool/offline_visualizer_tool/conf/"
-      "offline_lidar_perception_test.flag";
+      "./modules/perception/tool/export_sensor_data/conf/"
+      "export_sensor_data.flag";
   gflags::ParseCommandLineFlags(&argc, &argv, true);
   apollo::perception::ExportSensorData export_sensor_data;
   export_sensor_data.Init();

modules/perception/tool/offline_visualizer_tool/conf/BUILD

@@ -5,5 +5,6 @@ filegroup(
     srcs = [
         "config_manager.config",
         "offline_lidar_perception_test.flag",
+        "offline_sequential_obstacle_perception_test.flag",
     ],
 )

modules/perception/tool/offline_visualizer_tool/conf/offline_sequential_obstacle_perception_test.flag

+
+--work_root=modules/perception
+--config_manager_path=conf/config_manager.config
+
+--enable_global_offset=false
+
+####################################################################
+# Flags from obstacle/lidar/test/offline_lidar_perception_test.cpp
+# pcd path
+# type: string
+# default: /apollo/data/pcd/
+--lidar_path=./modules/perception/tool/export_sensor_data/lidar/
+--radar_path=./modules/perception/tool/export_sensor_data/radar/
+
+# pose path
+# type: string
+# default: /apollo/data/pose/
+--pose_path=/apollo/data/pose/
+
+# output path
+# type: string
+# default: /apollo/data/output/
+--save_obstacles=false
+--output_path=/apollo/data/output/
+
+# enable visualization
+# type: bool
+# default: true
+--enable_visualization=true
+--show_lidar_obstacles=false
+--show_radar_obstacles=false
+--show_fused_obstacles=true
+
+####################################################################
+# Flags from obstacle/onboard/hdmap_input.cc
+
+# roi distance of car center
+# type: double
+# default: 60.0
+--map_radius=60.0
+
+# step for sample road boundary points
+# type: int32
+# default: 1
+--map_sample_step=1
+
+--flagfile=modules/common/data/global_flagfile.txt
+
+####################################################################
+# Flags from obstacle/onboard/lidar_process.cc
+# enable hdmap input for roi filter
+# type: bool
+# default: false
+--enable_hdmap_input=true
+
+# roi filter before GroundDetector.
+# type: string
+# candidate: DummyROIFilter, HdmapROIFilter
+--onboard_roi_filter=HdmapROIFilter
+
+# the segmentation algorithm for onboard
+# type: string
+# candidate: DummySegmentation, CNNSegmentation
+--onboard_segmentor=CNNSegmentation
+
+# the object build algorithm for onboard
+# type: string
+# candidate: DummyObjectBuilder, MinBoxObjectBuilder
+--onboard_object_builder=MinBoxObjectBuilder
+
+# the tracking algorithm for onboard
+# type: string
+# candidate: DummyTracker, HmObjectTracker
+--onboard_tracker=HmObjectTracker
+
+--onboard_radar_detector=ModestRadarDetector
+
+# the perception module's output topic name.
+# type: string
+# default: perception_obstacle
+--obstacle_module_name=perception_obstacle
+
+# Query Ros TF timeout in ms. ros::Duration time.
+# type: int
+# default: 10
+--tf2_buff_in_ms=10
+
+# ros TF2 quary frame id. tf2_buffer.lookupTransform.
+# type: string
+# default: world
+--lidar_tf2_frame_id=world
+
+# ros TF2 quary child frame id. tf2_buffer.lookupTransform.
+# type: string
+# default: velodyne64
+--lidar_tf2_child_frame_id=velodyne64
+
+--v=4

modules/perception/tool/offline_visualizer_tool/offline_lidar_visualizer_tool.cc

@@ -124,7 +124,7 @@ class OfflineLidarPerceptionTool {
         content.SetLidarPose(pose);
         content.SetLidarCloud(cloud);
         content.SetLidarRoiCloud(roi_cloud);
-        content.SetTrackedObjectsLidar(result_objects);
+        content.SetTrackedObjects(result_objects);
         visualizer_->UpdateCameraSystem(&content);
         visualizer_->Render(content);
       }

modules/perception/tool/offline_visualizer_tool/offline_sequential_obstacle_perception_test.cc

@@ -29,12 +29,13 @@
 #include "pcl/io/pcd_io.h"
 
 DECLARE_string(flagfile);
-DEFINE_string(pcd_path, "./pcd/", "pcd path");
-DEFINE_string(radar_type, "radar_front", "");
-DEFINE_string(radar_path, "./radar_front/", "radar path");
-DEFINE_string(radar2velodyne_extrinsic, "./params/radar_front_extrinsics.yaml",
-              "radar2velodyne");
-DEFINE_bool(enable_global_offset, false,
+DEFINE_string(lidar_path,
+              "./modules/perception/tool/export_sensor_data/lidar/",
+              "lidar path");
+DEFINE_string(radar_path,
+              "./modules/perception/tool/export_sensor_data/radar/",
+              "radar path");
+DEFINE_bool(enable_global_offset, true,
             "enable global offset for benchmark evaluation");
 DEFINE_string(main_sensor, "velodyne_64", "main publish sensor");
 
@@ -53,7 +54,7 @@ struct SensorFile {
                                   const SensorFile& sensor_file) {
     out << "sensor_key: " << sensor_file.sensor_key
         << " file_path: " << sensor_file.file_path
-        << " timestamp: " << sensor_file.timestamp;
+        << std::setprecision(16) << " timestamp: " << sensor_file.timestamp;
     return out;
   }
 };
@@ -84,18 +85,14 @@ bool LoadOdometry(const std::string& filepath, Eigen::Vector3f* velocity) {
     return false;
   }
   double timestamp = 0.0;
-  double trans[3];
-  double quat[4];
-  Eigen::Vector3f temp;
-  fin >> timestamp >> trans[0] >> trans[1] >> trans[2]
-      >> quat[0] >> quat[1] >> quat[2] >> quat[3]
-      >> (*velocity)(0) >> (*velocity)(1) >> (*velocity)(2)
-      >> temp(0) >> temp(1) >> temp(2);
+  int frame_id;
+  fin >> frame_id >> timestamp
+      >> (*velocity)(0) >> (*velocity)(1) >> (*velocity)(2);
   bool state = true;
-  if (!fin.good()) {
-    state = false;
-    AERROR << "Failed to read odometry: " << filepath;
-  }
+  // if (!fin.good()) {
+  //   state = false;
+  //   AERROR << "Failed to read odometry: " << filepath;
+  // }
   fin.close();
   return state;
 }
@@ -124,39 +121,30 @@ class SequentialPerceptionTest {
     sensor_frame_reconstructor_["pcd"] = std::bind(
         &SequentialPerceptionTest::ReconstructPointcloudSensorRawFrame, this,
         std::placeholders::_1, std::placeholders::_2);
-    sensor_frame_reconstructor_[FLAGS_radar_type] =
+    sensor_frame_reconstructor_["radar"] =
         std::bind(&SequentialPerceptionTest::ReconstructRadarSensorRawFrame,
                   this, std::placeholders::_1, std::placeholders::_2);
     sensors_files_sources_ = {
-        {"pcd", SensorFilesSource(FLAGS_pcd_path, "pcd")},
-        {"radar_front", SensorFilesSource(FLAGS_radar_path, FLAGS_radar_type)}};
-
-    Eigen::Affine3d r2v_extrinsic;
-    if (!LoadExtrinsic(FLAGS_radar2velodyne_extrinsic, &r2v_extrinsic)) {
-      AERROR << "Failed to get radar2velodyne_extrinsic";
-      return false;
-    }
-    radar2velodyne_ex_ = r2v_extrinsic.matrix();
+        {"pcd", SensorFilesSource(FLAGS_lidar_path, "pcd")},
+        {"radar", SensorFilesSource(FLAGS_radar_path, "radar")}};
 
     return true;
   }
 
   bool ReconstructSensorRawFrame(const std::string& file_path,
-                                 std::shared_ptr<SensorRawFrame> frame) {
+                                 std::shared_ptr<SensorRawFrame>& frame) {
     std::string type = file_path.substr(file_path.find_last_of(".") + 1);
     auto find_res = sensor_frame_reconstructor_.find(type);
     if (find_res == sensor_frame_reconstructor_.end()) {
-      AERROR << "This kind file " << type << "is not supported";
-      AERROR << "Pass file: " << file_path;
+      AERROR << "The file type: " << type << ", is not supported";
       return false;
     }
     return (find_res->second)(file_path, frame);
   }
 
   bool ReconstructPointcloudSensorRawFrame(
-      const std::string& file_path, std::shared_ptr<SensorRawFrame> frame) {
-    frame.reset(new VelodyneRawFrame);
-    std::string type = file_path.substr(file_path.size() - 3, 3);
+      const std::string& file_path, std::shared_ptr<SensorRawFrame>& frame) {
+    std::string type = file_path.substr(file_path.find_last_of(".") + 1);
     if (type != "pcd") {
       AERROR
           << "reconstruct_pointcloud_sensor_raw_frame can only handle pcd file";
@@ -164,16 +152,19 @@ class SequentialPerceptionTest {
       return false;
     }
     // static Eigen::Matrix4d pose_original;
-    Eigen::Matrix4d pose;
     AINFO << "Process pcd";
+    // read pose and timestamp
+    Eigen::Matrix4d pose;
     int frame_id = 0;
     double timestamp = 0.0;
-    std::string pose_filename =
+    std::string pose_filename = FLAGS_lidar_path + "/" +
         file_path.substr(0, file_path.find_last_of('.')) + ".pose";
     if (!ReadPoseFile(pose_filename, &pose, &frame_id, &timestamp)) {
       AERROR << "Pose file not exits: " << pose_filename;
       return false;
     }
+    std::cout << "read pose file: " << std::endl;
+    std::cout << pose << std::endl;
     if (!init_offset_) {
       global_offset_ = pose.col(3).head(3);
       obstacle_perception_.SetGlobalOffset(global_offset_);
@@ -182,11 +173,13 @@ class SequentialPerceptionTest {
     pose.col(3).head(3) -= global_offset_;
 
     // pose_original = pose;
-    VelodyneRawFrame* velodyne_frame =
-        dynamic_cast<VelodyneRawFrame*>(frame.get());
+    // read pcd
     pcl::PointCloud<pcl_util::PointXYZIT>::Ptr cloud_raw(
         new pcl::PointCloud<pcl_util::PointXYZIT>);
-    pcl::io::loadPCDFile<pcl_util::PointXYZIT>(file_path, *cloud_raw);
+    std::string pcd_filename = FLAGS_lidar_path + "/" +
+        file_path.substr(0, file_path.find_last_of('.')) + ".pcd";
+    AINFO << "pcd file_name: " << pcd_filename;
+    pcl::io::loadPCDFile<pcl_util::PointXYZIT>(pcd_filename, *cloud_raw);
     pcl_util::PointCloudPtr cloud(new pcl_util::PointCloud);
     double min_timestamp = DBL_MAX;
     double max_timestamp = 0;
@@ -196,68 +189,74 @@ class SequentialPerceptionTest {
       p.y = cloud_raw->points[i].y;
       p.z = cloud_raw->points[i].z;
       p.intensity = cloud_raw->points[i].intensity;
-      // p.timestamp = cloud_raw->points[i].timestamp;
       cloud->points.push_back(p);
-      // min_timestamp = std::min<double>(p.timestamp, min_timestamp);
-      // max_timestamp = std::max<double>(p.timestamp, max_timestamp);
     }
-    AINFO << "Timestamp range : " << GLOG_TIMESTAMP(min_timestamp) << ", "
-          << GLOG_TIMESTAMP(max_timestamp) << ", " << GLOG_TIMESTAMP(timestamp);
 
     // pose = pose * _velodyne2novatel_ex;
+    frame.reset(new VelodyneRawFrame);
+    VelodyneRawFrame* velodyne_frame =
+        dynamic_cast<VelodyneRawFrame*>(frame.get());
     velodyne_frame->timestamp_ = timestamp;
     velodyne_frame->pose_ = pose;
     velodyne_frame->sensor_type_ = VELODYNE_64;
     velodyne_frame->cloud_ = cloud;
+    AINFO << "velo frame sensor_type_ = " << frame->sensor_type_;
     return true;
   }
 
   bool ReconstructRadarSensorRawFrame(const std::string& file_path,
-                                      std::shared_ptr<SensorRawFrame> frame) {
-    frame.reset(new RadarRawFrame);
-    std::string type = file_path.substr(file_path.size() - 5, 5);
-    if (type != FLAGS_radar_type) {
+                                      std::shared_ptr<SensorRawFrame>& frame) {
+    std::string type = file_path.substr(file_path.find_last_of(".") + 1);
+    if (type != "radar") {
       AERROR << "reconstruct_radar_sensor_raw_frame can only handle "
-             << FLAGS_radar_type << " file";
+             << "radar file";
       AERROR << "Pass file: " << file_path;
       return false;
     }
+
+    // read pose
     Eigen::Matrix4d pose;
-    AINFO << "Process " << FLAGS_radar_type;
     int frame_id = 0;
     double timestamp = 0.0;
+    std::string pose_filename = FLAGS_radar_path + "/" +
+        file_path.substr(0, file_path.find_last_of('.')) + ".pose";
+    if (!ReadPoseFile(pose_filename, &pose, &frame_id, &timestamp)) {
+      AERROR << "Pose file not exits: " << pose_filename;
+      return false;
+    }
+    // read radar obstacle
     ContiRadar radar_obs_proto;
-    Eigen::Vector3f velocity;
-    std::string radar_filename =
+    std::string radar_filename = FLAGS_radar_path + "/" +
         file_path.substr(0, file_path.find_last_of('.')) + ".radar";
-    std::string odometry_filename =
-        file_path.substr(0, file_path.find_last_of('.')) + ".odometry";
     if (!LoadRadarProto(radar_filename, &radar_obs_proto)) {
       AERROR << "Failed to load " << radar_filename;
       return false;
     }
+    AINFO << "radar_obs_proto size: " << radar_obs_proto.contiobs().size();
+    // read host vehicle velocity
+    Eigen::Vector3f velocity;
+    std::string odometry_filename = FLAGS_radar_path + "/" +
+        file_path.substr(0, file_path.find_last_of('.')) + ".velocity";
     if (!LoadOdometry(odometry_filename, &velocity)) {
       AERROR << "Failed to load " << odometry_filename;
       return false;
     }
-    std::string pose_filename =
-        file_path.substr(0, file_path.find_last_of('.')) + ".pose";
-    if (!ReadPoseFile(pose_filename, &pose, &frame_id, &timestamp)) {
-      AERROR << "Pose file not exits: " << pose_filename;
-      return false;
-    }
+
     if (!init_offset_) {
       global_offset_ = pose.col(3).head(3);
       obstacle_perception_.SetGlobalOffset(global_offset_);
       init_offset_ = true;
     }
     pose.col(3).head(3) -= global_offset_;
+
+    frame.reset(new RadarRawFrame);
     RadarRawFrame* radar_frame = dynamic_cast<RadarRawFrame*>(frame.get());
     radar_frame->timestamp_ = timestamp;
     radar_frame->pose_ = pose;
     radar_frame->sensor_type_ = RADAR;
     radar_frame->raw_obstacles_ = radar_obs_proto;
     radar_frame->car_linear_speed_ = velocity;
+    AINFO << "radar frame sensor_type_ = " << frame->sensor_type_;
     return true;
   }
 
@@ -268,6 +267,7 @@ class SequentialPerceptionTest {
     for (const auto& sensor_files_source : sensors_files_sources_) {
       std::string sensor_key = sensor_files_source.first;
       const SensorFilesSource& source = sensor_files_source.second;
+      AINFO << "source folder_path: " << source.folder_path;
       if (!apollo::common::util::DirectoryExists(source.folder_path)) {
         AWARN << "No sensor files source: " << sensor_key;
         continue;
@@ -280,8 +280,8 @@ class SequentialPerceptionTest {
           sensors_files_lists[sensor_key];
       sensor_files_list.reserve(files_list.size());
       for (const auto& file_name : files_list) {
-        std::string pose_file =
-            file_name.substr(0, file_name.find_last_of('.')) + ".pose";
+        std::string pose_file = source.folder_path + "/"
+            + file_name.substr(0, file_name.find_last_of('.')) + ".pose";
         FILE* fin = fopen(pose_file.c_str(), "rt");
         if (fin == nullptr) {
           AWARN << "pose file: " << pose_file
@@ -318,21 +318,9 @@ class SequentialPerceptionTest {
     PERF_BLOCK_END("get_sequential_sensors_data_files");
   }
 
-  void Run(SensorRawFrame* frame,
-           const std::string& output_file_name = std::string()) {
+  void Run(SensorRawFrame* frame) {
     std::vector<ObjectPtr> fused_objs;
     obstacle_perception_.Process(frame, &fused_objs);
-    if (GetSensorType(frame->sensor_type_) == FLAGS_main_sensor) {
-      Eigen::Matrix4d velodyne_pose;
-      if (FLAGS_main_sensor == "velodyne_64") {
-        velodyne_pose = frame->pose_;
-      } else if (FLAGS_main_sensor == "radar") {
-        velodyne_pose = (radar2velodyne_ex_ * frame->pose_.inverse()).inverse();
-      } else {
-        AERROR << "Unsupported main sensor type: " << FLAGS_main_sensor;
-        return;
-      }
-    }
   }
 
   const Eigen::Vector3d& GetGlobalOffset() {
@@ -344,8 +332,6 @@ class SequentialPerceptionTest {
   std::map<std::string, SensorFrameReconstructor> sensor_frame_reconstructor_;
   std::map<std::string, SensorFilesSource> sensors_files_sources_;
 
-  Eigen::Matrix4d radar2velodyne_ex_;
-
   ObstaclePerception obstacle_perception_;
   Eigen::Vector3d global_offset_;
   bool init_offset_;
@@ -355,7 +341,9 @@ class SequentialPerceptionTest {
 }  // namespace apollo
 
 int main(int argc, char** argv) {
-  FLAGS_flagfile = "./conf/perception_onboard.flag.cn";
+  FLAGS_flagfile =
+      "./modules/perception/tool/offline_visualizer_tool/conf/"
+      "offline_sequential_obstacle_perception_test.flag";
   google::ParseCommandLineFlags(&argc, &argv, true);
 
   apollo::perception::SequentialPerceptionTest test;
@@ -369,10 +357,10 @@ int main(int argc, char** argv) {
   AINFO << "Total sensors files num: " << sensors_files.size();
   for (size_t i = 0; i < sensors_files.size(); ++i) {
     AINFO << "Process frame " << sensors_files[i];
-    std::shared_ptr<apollo::perception::SensorRawFrame> raw_frame;
+    std::shared_ptr<apollo::perception::SensorRawFrame> raw_frame(
+                                  new apollo::perception::SensorRawFrame);
     test.ReconstructSensorRawFrame(sensors_files[i].file_path, raw_frame);
-    test.Run(raw_frame.get(), apollo::perception::GetFileName(
-             sensors_files[i].file_path));
+    test.Run(raw_frame.get());
   }
   return 0;
 }