adding gl_visualzer

modules/perception/common/perception_gflags.h

@@ -91,5 +91,11 @@ DECLARE_string(screen_output_dir);
 DECLARE_double(car_length);
 DECLARE_double(car_width);
 
+DECLARE_bool(show_front_radar_objects);
+DECLARE_bool(show_back_radar_objects);
+DECLARE_bool(show_camera_objects2d);
+DECLARE_bool(show_camera_parsing);
+DECLARE_string(frame_visualizer);
+
 #endif  // MODULES_PERCEPTION_COMMON_PERCEPTION_GFLAGS_H_
 

modules/perception/obstacle/camera/visualizer/BUILD

@@ -7,11 +7,13 @@ cc_library(
     srcs = [
         "frame_content.cc",
         "glfw_fusion_viewer.cc",
+        "gl_fusion_visualizer.cc",
     ],
     hdrs = [
         "frame_content.h",
         "base_visualizer.h",
         "glfw_fusion_viewer.h",
+        "gl_fusion_visualizer.h",
     ],
     copts = [
         "-Wno-deprecated",

modules/perception/obstacle/camera/visualizer/base_visualizer.h

@@ -72,7 +72,7 @@ class BaseVisualizer {
 
     virtual void update_camera_system(FrameContent *content) {}
 
-    virtual void render(const FrameContent &content) = 0;
+    virtual void render(FrameContent *content) = 0;
 
     // virtual void set_motion_buffer(MotionBuffer &motion_buffer) {}
 

modules/perception/obstacle/camera/visualizer/gl_fusion_visualizer.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/camera/visualizer/gl_fusion_visualizer.h"
+#include "modules/common/log.h"
+
+namespace apollo {
+namespace perception {
+namespace lowcostvisualizer {
+
+GLFusionVisualizer::GLFusionVisualizer() : _name("GLFusionVisualizer") {}
+
+bool GLFusionVisualizer::init() {
+  _opengl_vs = std::shared_ptr<GLFWFusionViewer>(new GLFWFusionViewer());
+
+  if (_opengl_vs == nullptr) {
+    AINFO << "Failed to create opengl viewer";
+    return false;
+  }
+
+  if (_opengl_vs->initialize() == false) {
+    AINFO << "Failed to initialize opengl viewer";
+    return false;
+  }
+  set_background_color(0.05, 0.05, 0.05, 0.0f);
+  set_camera_position();
+  set_main_car_points();
+  AINFO << "Initialize GLFusionVisualizer successfully";
+  return true;
+}
+
+void GLFusionVisualizer::render(FrameContent *content) {
+  _opengl_vs->set_camera_para(_camera_center_velodyne, _view_point_velodyne,
+                              _up_velodyne);
+  _opengl_vs->set_forward_dir(_forward_world);
+  _opengl_vs->set_main_car(_main_car_world);
+  _opengl_vs->set_frame_content(content);
+  //    _opengl_vs->set_motion_content(_motion_buffer);
+  _opengl_vs->spin_once();
+
+  AINFO << "GLFusionVisualizer spin_once";
+}
+
+void GLFusionVisualizer::set_background_color(float r, float g, float b,
+                                              float a) {
+  _opengl_vs->set_background_color(Eigen::Vector3d(r, g, b));
+}
+
+void GLFusionVisualizer::set_camera_position() {
+  _up_velodyne[0] = 0;
+  _up_velodyne[1] = 1;
+  _up_velodyne[2] = 0;
+  _forward_velodyne[0] = 1;
+  _forward_velodyne[1] = 0;
+  _forward_velodyne[2] = 0;
+  _view_point_velodyne[0] = 0;
+  _view_point_velodyne[1] = 0;
+  _view_point_velodyne[2] = 0;
+  _camera_center_velodyne[0] = 0;
+  _camera_center_velodyne[1] = 0;
+  _camera_center_velodyne[2] = 100;
+}
+
+void GLFusionVisualizer::set_main_car_points() {
+  _main_car.resize(4);
+  _main_car[0][0] = 2.0;
+  _main_car[0][1] = 1.0;
+  _main_car[0][2] = 0.0;
+
+  _main_car[1][0] = -2.0;
+  _main_car[1][1] = 1.0;
+  _main_car[1][2] = 0.0;
+
+  _main_car[2][0] = -2.0;
+  _main_car[2][1] = -1.0;
+  _main_car[2][2] = 0.0;
+
+  _main_car[3][0] = 2.0;
+  _main_car[3][1] = -1.0;
+  _main_car[3][2] = 0.0;
+}
+
+void GLFusionVisualizer::update_camera_system(FrameContent *content) {
+  Eigen::Matrix4d pose_v2w = content->get_opengl_camera_system_pose();
+
+  pose_v2w = Eigen::Matrix4d::Identity();
+  Eigen::Vector4d camera_center_w(_camera_center_velodyne[0],
+                                  _camera_center_velodyne[1],
+                                  _camera_center_velodyne[2], 0);
+  camera_center_w = pose_v2w * camera_center_w;
+  _camera_center_world[0] = camera_center_w[0];
+  _camera_center_world[1] = camera_center_w[1];
+  _camera_center_world[2] = camera_center_w[2];
+
+  Eigen::Vector4d view_point_w(_view_point_velodyne[0], _view_point_velodyne[1],
+                               _view_point_velodyne[2], 0);
+  view_point_w = pose_v2w * view_point_w;
+  _view_point_world[0] = view_point_w[0];
+  _view_point_world[1] = view_point_w[1];
+  _view_point_world[2] = view_point_w[2];
+
+  Eigen::Vector4d up_w(_up_velodyne[0], _up_velodyne[1], _up_velodyne[2], 0);
+
+  up_w = pose_v2w * up_w;
+  _up_world[0] = up_w[0];
+  _up_world[1] = up_w[1];
+  _up_world[2] = up_w[2];
+
+  Eigen::Vector4d fd_w(_forward_velodyne[0], _forward_velodyne[1],
+                       _forward_velodyne[2], 0);
+  fd_w = pose_v2w * fd_w;
+  _forward_world[0] = fd_w[0];
+  _forward_world[1] = fd_w[1];
+  _forward_world[2] = fd_w[2];
+
+  _main_car_world.resize(4);
+  for (size_t i = 0; i < 4; ++i) {
+    Eigen::Vector4d main_car_w(_main_car[i][0], _main_car[i][1],
+                               _main_car[i][2], 0.0);
+
+    main_car_w = pose_v2w * main_car_w;
+    _main_car_world[i][0] = main_car_w[0];
+    _main_car_world[i][1] = main_car_w[1];
+    _main_car_world[i][2] = main_car_w[2];
+  }
+}
+
+REGISTER_VISUALIZER(GLFusionVisualizer);
+
+}  // namespace lowcostvisualizer
+}  // namespace perception
+}  // namespace apollo

modules/perception/obstacle/camera/visualizer/gl_fusion_visualizer.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_CAMERA_VISUALIZER_GL_FUSION_VISUALIZER_H_
+#define MODULES_PERCEPTION_OBSTACLE_CAMERA_VISUALIZER_GL_FUSION_VISUALIZER_H_
+#include <sstream>
+#include <string>
+#include <vector>
+#include <memory>
+#include "modules/perception/obstacle/camera/visualizer/base_visualizer.h"
+#include "modules/perception/obstacle/camera/visualizer/frame_content.h"
+#include "modules/perception/obstacle/camera/visualizer/glfw_fusion_viewer.h"
+
+namespace apollo {
+namespace perception {
+namespace lowcostvisualizer {
+
+class GLFusionVisualizer : public BaseVisualizer {
+ public:
+  GLFusionVisualizer();
+
+  virtual ~GLFusionVisualizer() = default;
+
+  bool init() override;
+
+  std::string name() const override {
+    return _name;
+  }
+  //    void set_motion_buffer(MotionBuffer &motion_buffer) override {
+  //        _motion_buffer = motion_buffer;
+  //    }
+
+  void update_camera_system(FrameContent *content) override;
+
+  void render(FrameContent *content) override;
+
+ private:
+  void set_background_color(float r, float g, float b, float a);
+
+  void set_main_car_points();
+
+  void set_camera_position();
+
+  Eigen::Vector3d _camera_center_velodyne;
+  Eigen::Vector3d _view_point_velodyne;
+  Eigen::Vector3d _up_velodyne;
+  Eigen::Vector3d _forward_velodyne;
+  std::vector<Eigen::Vector3d> _main_car;
+
+  Eigen::Vector3d _camera_center_world;
+  Eigen::Vector3d _view_point_world;
+  Eigen::Vector3d _up_world;
+  Eigen::Vector3d _forward_world;
+  std::vector<Eigen::Vector3d> _main_car_world;
+
+  std::shared_ptr<GLFWFusionViewer> _opengl_vs;
+  std::string _name;
+  bool _init = false;
+  DISALLOW_COPY_AND_ASSIGN(GLFusionVisualizer);
+
+  //    boost::circular_buffer<VehicleStatus> _motion_buffer;
+};
+
+}  // namespace lowcostvisualizer
+}  // namespace perception
+}  // namespace apollo
+
+#endif  //  APOLLO_PERCEPTION_OBSTACLE_VISUALIZER_PCL_VISUALIZER_H