refactor visualizer

docker/dev.x86_64.dockerfile

@@ -157,3 +157,5 @@ RUN wget https://github.com/nigels-com/glew/releases/download/glew-2.0.0/glew-2.
 RUN unzip glew-2.0.0.zip
 WORKDIR /tmp/glew-2.0.0
 RUN make && make install
+RUN ln -s /usr/lib64/libGLEW.so /usr/lib/libGLEW.so
+RUN ln -s /usr/lib64/libGLEW.so.2.0 /usr/lib/libGLEW.so.2.0

docker/scripts/dev_start.sh

@@ -18,7 +18,7 @@
 
 VERSION=""
 ARCH=$(uname -m)
-VERSION_X86_64="dev-x86_64-20170803_1305"
+VERSION_X86_64="dev-x86_64-20170804_1205"
 VERSION_AARCH64="dev-aarch64-20170712_1533"
 if [[ $# == 1 ]];then
     VERSION=$1

modules/perception/obstacle/lidar/segmentation/cnnseg/cnn_segmentation.cc

@@ -126,7 +126,8 @@ bool CNNSegmentation::Segment(const pcl_util::PointCloudPtr& pc_ptr,
   // network forward process
   caffe_net_->Forward();
   network_time_ = timer_.Toc(true);
-
+  AERROR<< "objectness threshold is "<<cnnseg_param_.objectness_thresh();
+  AERROR<< "confidence threshold is "<<cnnseg_param_.confidence_thresh();
   // clutser points and construct segments/objects
   cluster2d_->Cluster(*category_pt_blob_, *instance_pt_blob_,
                       pc_ptr, valid_indices,

modules/perception/obstacle/lidar/visualizer/BUILD

-load("//tools:cpplint.bzl", "cpplint")
-
-load("//tools:cpplint.bzl", "cpplint")
-
-package(default_visibility = ["//visibility:public"])
-
-cc_library(
-    name = "perception_obstacle_lidar_visualizer",
-    srcs = [
-        "pcl_obstacle_visualizer.cc",
-        "pcl_vis_util.cc",
-        ],
-    hdrs = [
-        "pcl_obstacle_visualizer.h",
-        "pcl_vis_util.h",
-        ],
-    deps = [
-        "//modules/perception/lib/pcl_util:pcl_util",
-        "//modules/perception/obstacle/base:perception_obstacle_base",
-        "//modules/perception/lib/base:base",
-        "//modules/perception/obstacle/common:perception_obstacle_common",
-        "@eigen//:eigen",
-        "@pcl//:pcl",
-        "@vtk//:vtk",
-    ],
-)
-
-cpplint()

modules/perception/obstacle/lidar/visualizer/opengl_visualizer/BUILD

@@ -10,6 +10,7 @@ cc_library(
         "glfw_viewer.cc",
         "opengl_visualizer.cc",
         "frame_content.cc",
+        "pcl_vis_util.cc",
         ],
     hdrs = [
         "frame.h",
@@ -17,6 +18,7 @@ cc_library(
         "glfw_viewer.h",
         "opengl_visualizer.h",
         "frame_content.h",
+        "pcl_vis_util.h",
         ],
     deps = [
         "//modules/perception/lib/pcl_util:pcl_util",

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

@@ -116,8 +116,6 @@ bool GLFWViewer::initialize(){
     show_velocity = 1;
     show_polygon = 0;
     show_text = 1;
-
-
     return true;
 }
 
@@ -438,12 +436,15 @@ bool GLFWViewer::draw_objects(FrameContent* content , bool draw_cube, bool draw_
 
    	tracked_objects = content->get_tracked_objects();
 
+    std::cout<<"tracked objects size "<<tracked_objects.size()<<std::endl;
+
     for (size_t i = 0; i < tracked_objects.size(); ++i) {
         objects.push_back(tracked_objects[i]);
     }
 
 	float rgb[3];
 	if (draw_cube){
+        std::cout<<"draw cube enabled with objects size"<< objects.size()<<std::endl;
 		float verts[8][3];
 		int i = 0;
 		vec3 center;//x,y,z
@@ -462,6 +463,8 @@ bool GLFWViewer::draw_objects(FrameContent* content , bool draw_cube, bool draw_
             size.y = objects[i]->width;              
             size.z = objects[i]->height;
 
+            std::cout<<"objects info "<<objects[i]->ToString()<<std::endl;
+
 			float x1 = size.x/2;
             float x2 = 0 - x1;
 			float y1 = size.y/2;

modules/perception/obstacle/lidar/visualizer/pcl_vis_util.cc → modules/perception/obstacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "modules/perception/obstacle/lidar/visualizer/pcl_vis_util.h"
+#include "modules/perception/obstacle/lidar/visualizer/opengl_visualizer/pcl_vis_util.h"
 
 namespace apollo {
 namespace perception {

modules/perception/obstacle/lidar/visualizer/pcl_obstacle_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/lidar/visualizer/pcl_obstacle_visualizer.h"
-
-#include <gflags/gflags.h>
-
-namespace apollo {
-namespace perception {
-
-DEFINE_int32(visual_spin_rate, 1, "visual_spin_rate");
-
-int ObstacleTrackData::_s_max_queue_size = 300;
-
-ObstacleTrackData::ObstacleTrackData() {
-
-}
-
-ObstacleTrackData::~ObstacleTrackData() {
-
-}
-
-void ObstacleTrackData::add_obstacle(const ObjectPtr& obj) {
-    if (_history_data.size() >= (size_t)_s_max_queue_size) {
-        _history_data.pop_front();
-    }
-    ObjectPtr new_obj(new Object());
-    new_obj->clone(*obj);
-    _history_data.push_back(new_obj);
-    _consecutive_invisible_count = 0;
-    _updated = true;
-}
-
-PCLObstacleVisualizer::PCLObstacleVisualizer():
-        _name("ObsVisualizer"),
-        _object_color_type(OBJECT_COLOR_TRACK),
-        _capture_screen(false),
-        _output_directory("") {
-    initialize_pcl_visualizer();
-}
-
-PCLObstacleVisualizer::PCLObstacleVisualizer(
-        const std::string& name,
-        const OBJECT_COLOR_TYPE color_type,
-        const bool capture_screen,
-        const std::string& output_directory):
-        _name(name),
-        _object_color_type(color_type),
-        _capture_screen(capture_screen),
-        _output_directory(output_directory) {
-    initialize_pcl_visualizer();
-}
-
-PCLObstacleVisualizer::~PCLObstacleVisualizer() {
-
-}
-
-//default pcl visualizer initialization
-void PCLObstacleVisualizer::initialize_pcl_visualizer() {
-    _pcl_vs = boost::shared_ptr<pcl::visualization::PCLVisualizer>(
-        new pcl::visualization::PCLVisualizer(_name));
-    _view_port = 0;
-    _pcl_vs->addCoordinateSystem(1.0, _view_port);
-    set_size(2000, 1400);
-    set_background_color(0.05, 0.05, 0.05, 0.0f);
-    set_velodyne_height(2.5);
-    set_car_points();
-    set_camera_position();
-}
-
-void PCLObstacleVisualizer::set_size(int w, int h) {
-    _width = w;
-    _height = h;
-    _pcl_vs->setSize(w, h);
-}
-
-void PCLObstacleVisualizer::set_background_color(float r, float g, float b, float a) {
-    _pcl_vs->setBackgroundColor(r, g, b, a);
-}
-
-void PCLObstacleVisualizer::set_velodyne_height(float h) {
-    _velodyne_height = h;
-}
-
-void PCLObstacleVisualizer::set_camera_position() {
-    _up.x = 0;
-    _up.y = 1;
-    _up.z = 0;
-    _view_point.x = 0;
-    _view_point.y = 0;
-    _view_point.z = 0;
-    _camera_center.x = 0;
-    _camera_center.y = 0;
-    _camera_center.z = 150;
-    // _camera_center = _main_car_points_local.at(7);
-}
-
-void PCLObstacleVisualizer::set_car_points() {
-    _main_car_points_local.resize(9);
-    _main_car_points_local.at(0).x = 0;
-    _main_car_points_local.at(0).y = 0;
-    _main_car_points_local.at(0).z = 0;
-
-    _main_car_points_local.at(1).x = 0;
-    _main_car_points_local.at(1).y = 0;
-    _main_car_points_local.at(1).z = -_velodyne_height;
-    _main_car_points_local.at(2).x = 3;
-    _main_car_points_local.at(2).y = 0;
-    _main_car_points_local.at(2).z = -_velodyne_height;
-
-    _main_car_points_local.at(3).x = 2.5;
-    _main_car_points_local.at(3).y = 1.0;
-    _main_car_points_local.at(3).z = -_velodyne_height;
-    _main_car_points_local.at(4).x = 2.5;
-    _main_car_points_local.at(4).y = -1.0;
-    _main_car_points_local.at(4).z = -_velodyne_height;
-    _main_car_points_local.at(5).x = -2.5;
-    _main_car_points_local.at(5).y = -1.0;
-    _main_car_points_local.at(5).z = -_velodyne_height;
-    _main_car_points_local.at(6).x = -2.5;
-    _main_car_points_local.at(6).y = 1.0;
-    _main_car_points_local.at(6).z = -_velodyne_height;
-
-    _main_car_points_local.at(7).x = 0;
-    _main_car_points_local.at(7).y = 0;
-    _main_car_points_local.at(7).z = 160;
-    _main_car_points_local.at(8).x = -40;
-    _main_car_points_local.at(8).y = 0;
-    _main_car_points_local.at(8).z = 50;
-}
-
-Eigen::Vector3f PCLObstacleVisualizer::get_cls_color(int cls) {
-    Eigen::Vector3f rgb(255, 255, 255);
-    switch (cls) {
-        case 0:
-            rgb << 128, 0, 255; //紫
-            break;
-        case 1:
-            rgb << 0, 255, 255; //青
-            break;
-        case 2:
-            rgb << 255, 255, 0; //黄
-            break;
-        case 3:
-            rgb << 255, 128, 128; //赤
-            break;
-        case 4:
-            rgb << 0, 0, 255; //蓝
-            break;
-        case 5:
-            rgb << 0, 255, 0; //绿
-            break;
-        case 6:
-            rgb << 255, 128, 0; //橙
-            break;
-        case 7:
-            rgb << 255, 0, 0; //赤
-            break;
-    }
-    return rgb;
-}
-
-Eigen::Vector3f PCLObstacleVisualizer::get_track_color(int track_id) {
-    int cid = track_id % 124 + 1;
-    int r = cid % 5;
-    int g = ((cid - r) / 5) % 5;
-    int b = (cid - r - 5 * g) / 25;
-    float id2intesity[5] = {0, 128, 255, 64, 192};
-
-    return Eigen::Vector3f(id2intesity[r], id2intesity[g], id2intesity[b]);
-}
-
-void PCLObstacleVisualizer::add_object(ObjectPtr& obj, std::string id) {
-    Eigen::Vector3f coord_dir(1.0, 0.0, 0.0);
-    Eigen::Vector3f size_dir = obj->direction.cast<float>();
-    Eigen::Matrix3f rot = vector_rot_mat_2d_xy\
-            <Eigen::Vector3f, Eigen::Matrix3f>(size_dir, coord_dir);
-
-    Eigen::Vector3f cnt = obj->center.cast<float>();
-    Eigen::Vector3f size = Eigen::Vector3f(obj->length, obj->width, obj->height);
-    Eigen::Vector3f rgb = get_cls_color(obj->type) / 255;
-    add_bbox_3d<pcl_util::Point>(cnt, size, rot, rgb, _pcl_vs.get(), _view_port, id);
-}
-
-void PCLObstacleVisualizer::visualize_detection(
-        pcl_util::PointCloudPtr ground,
-        std::vector<ObjectPtr>& objects,
-        int frame_id) {
-
-    add_pointcloud<pcl_util::Point>(_pcl_vs.get(), ground, 180, 180, 180, 1, "ground", _view_port);
-    add_pointcloud<pcl_util::Point>(_pcl_vs.get(), \
-            _main_car_points_local.makeShared(), 255, 255, 255, 5, "car_points", _view_port);
-
-    for (size_t i = 0; i < objects.size(); ++i) {
-        std::ostringstream oss;
-        oss << "obj_" << i;
-        Eigen::Vector3f rgb(1, 0, 0);
-        add_pointcloud<pcl_util::Point>(_pcl_vs.get(), objects[i]->cloud, 255, 0,
-                            0, 3, oss.str(), _view_port);
-        add_object(objects[i], oss.str());
-    }
-    _pcl_vs.get()->setCameraPosition(_camera_center.x, _camera_center.y, _camera_center.z,
-        _view_point.x, _view_point.y, _view_point.z, _up.x, _up.y, _up.z);
-    _pcl_vs.get()->spinOnce();
-
-    std::ostringstream oss_prefix;
-    oss_prefix << std::setfill('0') << std::setw(6) << frame_id;
-    std::string file_name = _output_directory + "/detection_" + oss_prefix.str() + ".png";
-    std::cout << file_name << "\n";
-    _pcl_vs.get()->saveScreenshot(file_name);
-    _pcl_vs.get()->removeAllPointClouds();
-    _pcl_vs.get()->removeAllShapes();
-}
-
-void PCLObstacleVisualizer::visualize_tracking(
-        pcl_util::PointDCloudPtr cloud_world,
-        std::vector<ObjectPtr>& objects_world,
-        const Eigen::Matrix4d& pose_velo2w,
-        int frame_id,
-        const pcl_util::PointIndices& roi_indices) {
-    Eigen::Matrix4d pose = pose_velo2w;
-    if (_track_data.empty()) {
-        _global_offset = Eigen::Vector3d(-pose_velo2w(0, 3),
-            -pose_velo2w(1, 3), -pose_velo2w(2, 3));
-    }
-    pose(0, 3) += _global_offset[0];
-    pose(1, 3) += _global_offset[1];
-    pose(2, 3) += _global_offset[2];
-
-    for (size_t i = 0; i < cloud_world->size(); i++) {
-        cloud_world->points[i].x += _global_offset[0];
-        cloud_world->points[i].y += _global_offset[1];
-        cloud_world->points[i].z += _global_offset[2];
-    }
-
-    for (size_t i = 0; i <objects_world.size(); i++) {
-        objects_world[i]->center += _global_offset;
-        for (size_t j = 0; j < objects_world[i]->cloud->size(); j++) {
-            objects_world[i]->cloud->points[j].x += _global_offset[0];
-            objects_world[i]->cloud->points[j].y += _global_offset[1];
-            objects_world[i]->cloud->points[j].z += _global_offset[2];
-        }
-        for (size_t j = 0; j < objects_world[i]->polygon.size(); j++) {
-            objects_world[i]->polygon.points[j].x += _global_offset[0];
-            objects_world[i]->polygon.points[j].y += _global_offset[1];
-            objects_world[i]->polygon.points[j].z += _global_offset[2];
-        }
-    }
-
-    /// transform camera center to world
-    pcl_util::Point camera_center_w;
-    transform_point_cloud<pcl_util::Point>(_camera_center, camera_center_w, pose);
-    /// transform camera view point to world
-    pcl_util::Point camera_vpoint_w;
-    transform_point_cloud<pcl_util::Point>(_view_point, camera_vpoint_w, pose);
-    /// transform host car points to world
-    pcl_util::PointCloudPtr w_car_points(new pcl_util::PointCloud);
-    transform_point_cloud<pcl_util::Point>(_main_car_points_local, *w_car_points, pose);
-    /// show and save tracking result
-    Eigen::Vector4d up_w(_up.x, _up.y, _up.z, 0);
-    up_w = pose * up_w;
-
-    std::ostringstream oss_prefix;
-    oss_prefix << std::setfill('0') << std::setw(6) << frame_id;
-    std::string file_name = _output_directory + "/tracking_" + oss_prefix.str() + ".png";
-
-    pcl_util::PointDCloudPtr roi_cloud(new pcl_util::PointDCloud);
-    pcl::copyPointCloud(*cloud_world, roi_indices, *roi_cloud);
-
-    add_pointcloud<pcl_util::PointD>(_pcl_vs.get(), \
-            cloud_world, 180, 180, 180, 1, "ground", _view_port);
-    add_pointcloud<pcl_util::Point>(_pcl_vs.get(), \
-            w_car_points, 255, 255, 255, 5, "car_points", _view_port);
-    add_pointcloud<pcl_util::PointD>(_pcl_vs.get(), \
-            roi_cloud, 0, 0, 205, 1, "roi", _view_port); // MediumBlue
-
-    for (std::map<int, ObstacleTrackData>::iterator it = _track_data.begin();
-        it != _track_data.end(); ++it) {
-        it->second._updated = false;
-    }
-
-    for (size_t i = 0; i < objects_world.size(); i++) {
-        int track_id = objects_world[i]->track_id;
-        std::map<int, ObstacleTrackData>::iterator it = _track_data.find(track_id);
-        if (it == _track_data.end()) {
-            ObstacleTrackData& data = _track_data[track_id];
-            data.add_obstacle(objects_world[i]);
-        } else {
-            it->second.add_obstacle(objects_world[i]);
-        }
-    }
-
-    for (std::map<int, ObstacleTrackData>::iterator it = _track_data.begin();
-        it != _track_data.end();) {
-        if (it->second._updated == false) {
-            it->second._consecutive_invisible_count++;
-            if (it->second._consecutive_invisible_count > 1) {
-                it = _track_data.erase(it);
-            } else {
-                ++it;
-            }
-        } else {
-            ++it;
-        }
-    }
-
-    for (std::map<int, ObstacleTrackData>::iterator it = _track_data.begin();
-        it != _track_data.end(); ++it) {
-        if (it->second._consecutive_invisible_count > 0) {
-            continue;
-        }
-
-        std::ostringstream oss;
-        oss << "track_" << it->first;
-        std::string id = oss.str() + "_cloud";
-        ObjectPtr recent_obj = it->second._history_data.back();
-        add_pointcloud<pcl_util::Point>(_pcl_vs.get(),
-            recent_obj->cloud, 255, 0, 3, 1, id, _view_port);
-
-        add_object(recent_obj, oss.str());
-
-        pcl_util::Point src;
-        pcl_util::Point dst;
-        src.x = recent_obj->center[0];
-        src.y = recent_obj->center[1];
-        src.z = recent_obj->center[2];
-
-        dst.x = src.x + recent_obj->velocity[0];
-        dst.y = src.y + recent_obj->velocity[1];
-        dst.z = src.z + recent_obj->velocity[2];
-
-        id = oss.str() + "_velocity";
-        _pcl_vs.get()->addLine(src, dst, 1, 1, 1, id);
-
-        std::vector<Eigen::Vector3d> history_centers;
-        std::deque<ObjectPtr>::iterator dq_it = it->second._history_data.begin();
-        for (; dq_it != it->second._history_data.end(); dq_it++) {
-            history_centers.push_back((*dq_it)->center);
-        }
-
-        Eigen::Vector3f rgb(1, 1, 0);
-        int history_length = history_centers.size();
-        for (int j = 0; j < history_length - 1; j++) {
-            std::ostringstream id_oss;
-            id_oss << "_edge_" << j;
-            std::string tag = oss.str() + id_oss.str();
-            src.x = history_centers[j][0];
-            src.y = history_centers[j][1];
-            src.z = history_centers[j][2];
-
-            dst.x = history_centers[j + 1][0];
-            dst.y = history_centers[j + 1][1];
-            dst.z = history_centers[j + 1][2];
-            _pcl_vs.get()->addLine(src, dst, rgb[0], rgb[1], rgb[2], tag);
-        }
-        pcl_util::Point loc;
-        loc.x = recent_obj->center[0];
-        loc.y = recent_obj->center[1];
-        loc.z = recent_obj->center[2];
-        std::string text_id = oss.str();
-        std::ostringstream text_oss;
-        text_oss << recent_obj->track_id;
-        std::string disp_text = text_oss.str();
-        _pcl_vs.get()->addText3D(disp_text, loc, 0.5, 1, 1, 1, text_id + "_text", _view_port);
-    }
-
-    _pcl_vs.get()->setCameraPosition(camera_center_w.x, camera_center_w.y, camera_center_w.z,
-            camera_vpoint_w.x, camera_vpoint_w.y, camera_vpoint_w.z, up_w[0], up_w[1], up_w[2]);
-
-    _pcl_vs.get()->spinOnce(FLAGS_visual_spin_rate);
-    _pcl_vs.get()->saveScreenshot(file_name);
-    _pcl_vs.get()->removeAllPointClouds();
-    _pcl_vs.get()->removeAllShapes();
-}
-
-}  // namespace perception
-}  // namespace apollo

modules/perception/obstacle/lidar/visualizer/pcl_obstacle_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_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H
-#define MODULES_PERCEPTION_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H
-
-#include <deque>
-#include <iomanip>
-#include <string>
-#include <sstream>
-#include <vector>
-
-#include <boost/shared_ptr.hpp>
-#include <pcl/visualization/pcl_visualizer.h>
-
-#include "modules/perception/lib/pcl_util/pcl_types.h"
-#include "modules/perception/obstacle/base/object.h"
-#include "modules/perception/obstacle/lidar/visualizer/pcl_vis_util.h"
-
-namespace apollo {
-namespace perception {
-
-enum OBJECT_COLOR_TYPE{
-    OBJECT_COLOR_CLASSIFICATION = 0,
-    OBJECT_COLOR_TRACK
-};
-
-class ObstacleTrackData{
-public:
-    ObstacleTrackData();
-    ~ObstacleTrackData();
-
-    void add_obstacle(const ObjectPtr& obj);
-
-public:
-    std::deque<ObjectPtr>       _history_data;
-    int                         _consecutive_invisible_count;
-    bool                        _updated;
-private:
-    static int                  _s_max_queue_size;
-};
-
-class PCLObstacleVisualizer{
-public:
-    PCLObstacleVisualizer();
-    explicit PCLObstacleVisualizer(
-        const std::string& name,
-        const OBJECT_COLOR_TYPE color_type,
-        const bool capture_screen,
-        const std::string& output_directory);
-    ~PCLObstacleVisualizer();
-
-    void set_size(int w, int h);
-    void set_background_color(float r, float g, float b, float a = 1.0f);
-    void set_velodyne_height(float height);
-    void set_car_points();
-    void set_camera_position();
-    void set_output_dir(const std::string& outptut_dir) {
-        _output_directory = outptut_dir;
-    }
-
-    void visualize_detection(pcl_util::PointCloudPtr ground,
-        std::vector<ObjectPtr>& objects, int frame_id = -1);
-
-    void visualize_tracking(
-            pcl_util::PointDCloudPtr cloud_world,
-            std::vector<ObjectPtr>& objects_world,
-        const Eigen::Matrix4d& pose_velo2w,
-        int frame_id,
-        const pcl_util::PointIndices& roi_indices);
-
-protected:
-    void initialize_pcl_visualizer();
-    Eigen::Vector3f get_cls_color(int cls);
-    Eigen::Vector3f get_track_color(int track_id);
-
-    void add_object(ObjectPtr& obj, std::string id);
-
-private:
-    boost::shared_ptr<pcl::visualization::PCLVisualizer> _pcl_vs;
-
-    std::map<int, ObstacleTrackData> _track_data;
-
-    std::string _name;
-    int _view_port;
-
-    pcl_util::Point _camera_center;
-    pcl_util::Point _view_point;
-    pcl_util::Point _up;
-
-    float _velodyne_height;
-    pcl_util::PointCloud _main_car_points_local;
-
-    int _width;
-    int _height;
-
-    OBJECT_COLOR_TYPE _object_color_type;
-    bool _capture_screen;
-    std::string _output_directory;
-
-    Eigen::Vector3d _global_offset;
-};
-
-}  // namespace perception
-}  // namespace apollo
-#endif  // MODULES_PERCEPTION_VISUALIZATION_PCL_OBSTACLE_VISUALIZER_H

modules/perception/obstacle/onboard/lidar_process.cc

@@ -169,6 +169,7 @@ bool LidarProcess::Process(double timestamp, PointCloudPtr point_cloud,
   }
   AERROR << "call object_builder succ.";
   PERF_BLOCK_END("lidar_object_builder");
+  //objects_ = objects; 
 
   /// call tracker
   if (tracker_ != nullptr) {

modules/perception/tool/offline_lidar_perception_tool.cc

@@ -201,8 +201,8 @@ public:
 
                 pcl_util::PointIndices roi_indices_1;
                 FrameContent content;
-                content.set_lidar_cloud(cloud);
                 content.set_lidar_pose(pose);
+                content.set_lidar_cloud(cloud);
                 content.set_tracked_objects(result_objects);
                 visualizer_->update_camera_system(&content);
                 visualizer_->render(content);

modules/perception/tool/offline_visualizer_tool/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_binary(
+    name = "offline_lidar_visualizer_tool",
+    srcs = ["offline_lidar_visualizer_tool.cc"],
+    data = [      
+        "//modules/perception/tool/offline_visualizer_tool/conf:perception_tool_config",
+        "//modules/perception:perception_model",
+    ],
+    linkstatic = 0,
+    deps = [
+        "//modules/common",
+        "//modules/common:log",
+        "//modules/perception/common:perception_common",
+        "//modules/perception/lib/base",
+        "//modules/perception/lib/config_manager",
+        "//modules/perception/lib/pcl_util",
+        "//modules/perception/obstacle/common:perception_obstacle_common",
+        "//modules/perception/obstacle/lidar/visualizer/opengl_visualizer:perception_obstacle_lidar_opengl_visualizer",
+        "//modules/perception/obstacle/onboard:perception_obstacle_lidar_process",
+        "@eigen//:eigen",
+        "@glew//:glew",
+        "@glfw//:glfw",
+        "@opengl//:opengl",
+    ],
+)
+
+cpplint()

modules/perception/tool/offline_visualizer_tool/offline_lidar_visualizer_tool.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 <fstream>
+#include <iomanip>
+#include <sstream>
+#include <vector>
+#include <string>
+
+#include <pcl/io/pcd_io.h>
+
+#include "modules/perception/common/perception_gflags.h"
+#include "modules/perception/lib/config_manager/config_manager.h"
+#include "modules/perception/lib/pcl_util/pcl_types.h"
+#include "modules/perception/obstacle/common/file_system_util.h"
+#include "modules/perception/obstacle/onboard/lidar_process.h"
+#include "modules/perception/obstacle/lidar/visualizer/opengl_visualizer/opengl_visualizer.h"
+#include "modules/perception/obstacle/lidar/visualizer/opengl_visualizer/frame_content.h"
+
+DECLARE_string(flagfile);
+DECLARE_string(config_manager_path);
+DEFINE_string(pcd_path, "./pcd/", "pcd path");
+DEFINE_string(pose_path, "./pose/", "pose path");
+DEFINE_bool(enable_visualization, true, "enable visualization");
+
+namespace apollo {
+namespace perception {
+
+DEFINE_string(output_path, "./output/", "output path");
+DEFINE_int32(start_frame, 1000, "start frame");
+
+template <typename T>
+void quaternion_to_rotation_matrix(const T * quat, T * R) {
+    T x2 = quat[0] * quat[0];
+    T xy = quat[0] * quat[1];
+    T rx = quat[3] * quat[0];
+    T y2 = quat[1] * quat[1];
+    T yz = quat[1] * quat[2];
+    T ry = quat[3] * quat[1];
+    T z2 = quat[2] * quat[2];
+    T zx = quat[2] * quat[0];
+    T rz = quat[3] * quat[2];
+    T r2 = quat[3] * quat[3];
+    R[0] = r2 + x2 - y2 - z2;         // fill diagonal terms
+    R[4] = r2 - x2 + y2 - z2;
+    R[8] = r2 - x2 - y2 + z2;
+    R[3] = 2 * (xy + rz);             // fill off diagonal terms
+    R[6] = 2 * (zx - ry);
+    R[7] = 2 * (yz + rx);
+    R[1] = 2 * (xy - rz);
+    R[2] = 2 * (zx + ry);
+    R[5] = 2 * (yz - rx);
+}
+
+bool read_pose_file(const std::string& filename, Eigen::Matrix4d& pose,
+    int& frame_id, double& time_stamp) {
+    std::ifstream ifs(filename.c_str());
+    if (!ifs.is_open()) {
+        std::cerr << "Failed to open file " << filename << std::endl;
+        return false;
+    }
+    char buffer[1024];
+    ifs.getline(buffer, 1024);
+    int id = 0;
+    double time_samp = 0;
+    double quat[4];
+    double matrix3x3[9];
+    sscanf(buffer, "%d %lf %lf %lf %lf %lf %lf %lf %lf", &id, &(time_samp),
+            &(pose(0, 3)), &(pose(1, 3)), &(pose(2, 3)),
+            &(quat[0]), &(quat[1]), &(quat[2]), &(quat[3]));
+    quaternion_to_rotation_matrix<double>(quat, matrix3x3);
+
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            pose(i, j) = matrix3x3[i * 3 + j];
+        }
+    }
+
+    frame_id = id;
+    time_stamp = time_samp;
+    return true;
+}
+
+bool read_pose_file_mat12(const std::string& filename, Eigen::Matrix4d& pose,
+    int& frame_id, double& time_stamp) {
+    std::ifstream ifs(filename.c_str());
+    if (!ifs.is_open()) {
+        std::cerr << "Failed to open file " << filename << std::endl;
+        return false;
+    }
+    pose = Eigen::Matrix4d::Identity();
+    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;
+}
+
+class OfflineLidarPerceptionTool {
+public:
+    OfflineLidarPerceptionTool() {
+        config_manager_ = NULL;
+    };
+
+    ~OfflineLidarPerceptionTool() = default;
+
+    bool Init(bool use_visualization = false) {
+        FLAGS_config_manager_path = "tool/offline_visualizer_tool/conf/config_manager.config";
+        config_manager_ = Singleton<ConfigManager>::Get();
+       
+        if (config_manager_ == NULL || !config_manager_->Init()) {
+            AERROR << "failed to init ConfigManager";
+            return false;
+        }
+
+        lidar_process_.reset(new LidarProcess());
+
+        if (!lidar_process_->Init()) {
+          AERROR << "failed to init lidar_process.";
+          return false;
+        }
+
+        output_dir_ = FLAGS_output_path;
+
+        if (use_visualization) {
+            //visualizer_.reset(new PCLObstacleVisualizer("obstaclevisualizer_",
+            //    OBJECT_COLOR_TRACK, false, output_dir_));
+            visualizer_.reset(new OpenglVisualizer());
+            if(!visualizer_->init()) {
+                AERROR<<"init visialuzer failed"<<std::endl;
+            }; 
+        }
+        return true;
+    }
+
+    void Run(const std::string& pcd_path, const std::string pose_path) {
+
+        std::string pcd_folder = pcd_path;
+        std::string pose_folder = pose_path;
+        std::vector<std::string> pcd_file_names;
+        std::vector<std::string> pose_file_names;
+        AERROR << "starting to run"; 
+        get_file_names_in_folder(pose_folder, ".pose", pose_file_names);
+        get_file_names_in_folder(pcd_folder, ".pcd", pcd_file_names);
+        AERROR<<" pose size " << pose_file_names.size();
+        AERROR<<" pcd size " << pcd_file_names.size(); 
+        if (pose_file_names.size() != pcd_file_names.size()) {
+            AERROR << "pcd file number does not match pose file number";
+            return;
+        }
+        double time_stamp = 0.0;
+        int start_frame = FLAGS_start_frame;
+        AERROR << "starting frame is "<<start_frame;
+        sleep(1);
+        for (size_t i = start_frame; i < pcd_file_names.size(); i++) {
+            AERROR << "***************** Frame " << i << " ******************";
+            std::ostringstream oss;
+            pcl_util::PointCloudPtr cloud(new pcl_util::PointCloud);
+            AERROR << "load pcd file from file path" << pcd_folder + pcd_file_names[i];
+            pcl::io::loadPCDFile<pcl_util::Point>(pcd_folder + pcd_file_names[i], *cloud);
+            AERROR << "read point cloud from " << pcd_file_names[i]
+                << " with cloud size: " << cloud->points.size();
+
+            //read pose
+            Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
+            int frame_id = -1;
+            if (!read_pose_file(pose_folder + pose_file_names[i],
+                pose, frame_id, time_stamp)) {
+                std::cout << "Failed to read file " << pose_file_names[i] << "\n";
+                return ;
+            }
+            AERROR << "read pose file " << pose_file_names[i] << "  " << pose ;
+            
+            std::shared_ptr<Eigen::Matrix4d> velodyne_trans = std::make_shared<Eigen::Matrix4d>(pose);
+            lidar_process_->Process(time_stamp, cloud, velodyne_trans);
+
+            std::vector<ObjectPtr> result_objects = lidar_process_->GetObjects();
+            const pcl_util::PointIndicesPtr roi_indices = lidar_process_->GetROIIndices();
+
+            AERROR << "finish processing point cloud with num objects" << result_objects.size(); 
+            
+            if (visualizer_) {
+                pcl_util::PointDCloudPtr transformed_cloud(new pcl_util::PointDCloud);
+                transform_perception_cloud(cloud, pose, transformed_cloud);
+                AERROR<<"transformed cloud size is "<<transformed_cloud->size();
+
+                pcl_util::PointIndices roi_indices_1;
+                FrameContent content;
+                content.set_lidar_pose(pose);
+                content.set_lidar_cloud(cloud);
+                content.set_tracked_objects(result_objects);
+                visualizer_->update_camera_system(&content);
+                visualizer_->render(content);
+            }
+            AERROR << "finish pc";
+            //oss << std::setfill('0') << std::setw(6) << i;
+            //std::string filename = FLAGS_output_path + oss.str() + ".txt";
+            //save_obstacle_informaton(result_objects, filename);
+        }
+    }
+
+    void save_obstacle_informaton(
+        std::vector<ObjectPtr>& objects, std::string file_name) {
+        std::cout << "save " << objects.size() << " objects to "
+            << file_name << std::endl;
+        std::fstream test_file;
+        test_file.open(file_name.c_str(), std::fstream::out);
+        std::cout << "sort each object \n";
+        for (size_t i = 0; i < objects.size(); ++i) {
+            std::cout << i << " ";
+            ObjectPtr obs = objects[i];
+            PolygonDType& cloud = obs->polygon;
+            if (cloud.size() == 0) {
+                continue;
+            }
+            for (size_t j = 0; j < cloud.points.size() - 1; ++j) {
+                for (size_t k = j + 1; k < cloud.points.size(); ++k) {
+                    pcl_util::PointD p1 = cloud.points[j];
+                    pcl_util::PointD p2 = cloud.points[k];
+                    bool need_change = false;
+                    if (static_cast<long long>(10000 * p1.x) ==
+                        static_cast<long long>(10000 * p2.x)) {
+                        if (static_cast<long long>(10000 * p1.y)
+                                == static_cast<long long>(10000 * p2.y)) {
+                            need_change = static_cast<long long>(
+                                    10000 * p1.z) > static_cast<long long>(10000 * p2.z);
+                        } else {
+                            need_change = static_cast<long long>(
+                                    10000 * p1.y) > static_cast<long long>(10000 * p2.y);
+                        }
+                    } else {
+                        need_change = static_cast<long long>(
+                                10000 * p1.x) > static_cast<long long>(10000 * p2.x);
+                    }
+                    if (need_change) {
+                        pcl_util::PointD tmp = cloud.points[j];
+                        cloud.points[j] = cloud.points[k];
+                        cloud.points[k] = tmp;
+                    }
+                }
+            }
+        }
+        std::cout << "\nsort all objects\n";
+        for (int i = 0; i < (int)objects.size() - 1; ++i) {
+            std::cout << "\r" << i;
+            for (size_t j = i + 1; j < objects.size(); ++j) {
+                bool need_change = false;
+                ObjectPtr obs_i = objects[i];
+                ObjectPtr obs_j = objects[j];
+                PolygonDType& cloud_i = obs_i->polygon;
+                PolygonDType& cloud_j = obs_j->polygon;
+                if (cloud_i.points.size() == cloud_j.points.size()) {
+                    if (cloud_i.points.size() == 0) {
+                        continue;
+                    }
+                    pcl_util::PointD p1 = cloud_i.points[0];
+                    pcl_util::PointD p2 = cloud_j.points[0];
+                    if (static_cast<long long>(10000 * p1.x) ==
+                        static_cast<long long>(10000 * p2.x)) {
+                        if (static_cast<long long>(10000 * p1.y)
+                                == static_cast<long long>(10000 * p2.y)) {
+                            need_change = static_cast<long long>(
+                                    10000 * p1.z) > static_cast<long long>(10000 * p2.z);
+                        } else {
+                            need_change = static_cast<long long>(
+                                    10000 * p1.y) > static_cast<long long>(10000 * p2.y);
+                        }
+                    } else {
+                        need_change = static_cast<long long>(
+                                10000 * p1.x) > static_cast<long long>(10000 * p2.x);
+                    }
+                } else {
+                    need_change = cloud_i.points.size() > cloud_j.points.size();
+                }
+                if (need_change) {
+                    ObjectPtr obj_tmp = objects[i];
+                    objects[i] = objects[j];
+                    objects[j] = obj_tmp;
+                }
+            }
+        }
+        std::cout << "\nstart to save objects\n";
+        for (size_t i = 0; i < objects.size(); ++i) {
+            ObjectPtr obs = objects[i];
+            if (obs->polygon.size() <= 0) {
+                std::cout << "Find object with empty convex hull "
+                    << obs->cloud->size() << "\n";
+                continue;
+            }
+            // id
+            test_file << obs->track_id << "  ";
+            // test_file << obs.get_idx() << " ";
+            // position
+            test_file << std::setprecision(4) << std::fixed << obs->center[0]
+                << " " << std::setprecision(4) << std::fixed << obs->center[1]
+                << " " << std::setprecision(4) << std::fixed << obs->center[2] << " ";
+            // theta
+            double theta = 0;
+            Eigen::Vector3d dir = obs->direction;
+            if (fabs(dir[0]) < DBL_MIN) {
+                if (dir[1] > 0) {
+                    theta = M_PI / 2.0;
+                } else {
+                    theta = - M_PI / 2.0;
+                }
+            } else {
+                theta = atan(dir[1] / dir[0]);
+            }
+            test_file << std::setprecision(4) << theta << std::fixed << " ";
+
+            // velocity
+            test_file << std::setprecision(4) << std::fixed << obs->velocity[0] << std::fixed << " "
+                    << std::setprecision(4) << obs->velocity[1] << std::fixed << " "
+                    << std::setprecision(4) << obs->velocity[2] << std::fixed << " ";
+
+            // size
+            test_file << std::setprecision(4) << std::fixed << obs->length
+                << std::fixed << " " << std::setprecision(4) << obs->width
+                << std::fixed << " " << std::setprecision(4) << obs->height << std::fixed << " ";
+            // dir
+            // test_file << std::setprecision(4) << obs.get_dir()[0] << fixed << " " << std::setprecision(4) << obs.get_dir()[1] << fixed << " ";
+            // polygon
+            PolygonDType& cloud = obs->polygon;
+            test_file << cloud.size() << " ";
+            for (size_t j = 0; j < cloud.size(); ++j) {
+                test_file << std::setprecision(4) << std::fixed << cloud.points[j].x << std::fixed << " "
+                        << std::setprecision(4) << cloud.points[j].y << std::fixed << " "
+                        << std::setprecision(4) << cloud.points[j].z << std::fixed << " ";
+            }
+            // type
+            std::string type = "unknow";
+            if (obs->type == UNKNOWN) {
+                type = "unknow";
+            } else if (obs->type == PEDESTRIAN) {
+                type = "pedestrian";
+            } else if (obs->type == VEHICLE) {
+                type = "car";
+            }
+            test_file << type << " ";
+            test_file << std::endl;
+        }
+    }
+
+protected:
+    ConfigManager* config_manager_;
+    std::unique_ptr<LidarProcess> lidar_process_;
+    //std::unique_ptr<PCLObstacleVisualizer> visualizer_;
+    std::unique_ptr<OpenglVisualizer> visualizer_;
+    std::string output_dir_;
+    std::string pose_dir_;
+    std::string label_file_;
+};
+
+}  // namespace perception
+}  // namespace apollo
+
+
+int main(int argc, char* argv[]) {
+    FLAGS_flagfile = "./modules/perception/tool/offline_visualizer_tool/conf/offline_lidar_perception_test.flag";
+    gflags::ParseCommandLineFlags(&argc, &argv, true);
+    apollo::perception::OfflineLidarPerceptionTool tool;
+    tool.Init(FLAGS_enable_visualization);
+    tool.Run(FLAGS_pcd_path, FLAGS_pose_path);
+    return 0;
+}