Skip to content

Commit

Permalink
refine 3d motion and plot trajecktory (#4150)
Browse files Browse the repository at this point in the history
  • Loading branch information
@gchen-apollo
gchen-apollo authored May 12, 2018
1 parent b92894b commit 8a75dba
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 20 deletions.
4 changes: 2 additions & 2 deletions modules/perception/conf/dag_camera_obstacle_lane_motion_vis.config
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@ subnode_config {
subnodes {
id: 41
name: "VisualizationSubnode"
reserve: "vis_driven_event_id:1012;camera_event_id:1008;lane_event_id:1012"
# reserve: "vis_driven_event_id:1012;camera_event_id:1008;motion_event_id:1020;lane_event_id:1012"
# reserve: "vis_driven_event_id:1012;camera_event_id:1008;lane_event_id:1012"
reserve: "vis_driven_event_id:1012;camera_event_id:1008;motion_event_id:1020;lane_event_id:1012"
type: SUBNODE_OUT
}
}
Expand Down
46 changes: 33 additions & 13 deletions modules/perception/obstacle/camera/visualizer/glfw_fusion_viewer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -585,6 +585,11 @@ void GLFWFusionViewer::render() {
lane_objects_ =
std::make_shared<LaneObjects>(frame_content_->get_lane_objects());
if (draw_lane_objects_) {
const MotionBuffer& motion_buffer = frame_content_->get_motion_buffer();
int n = motion_buffer.size();
if (n > 0) {
motion_matrix_ = motion_buffer[n-1].motion;
}
draw_lane_objects_ground();
}
}
Expand Down Expand Up @@ -1023,6 +1028,7 @@ void GLFWFusionViewer::draw_lane_objects_ground() {
// }
while (lane_history_->size() < lane_objects_->size()) {
lane_history_->push_back(LaneObject());
z_history_.push_back(std::vector<float>());
}
}
for (size_t k = 0; k < lane_objects_->size(); ++k) {
Expand All @@ -1033,37 +1039,47 @@ void GLFWFusionViewer::draw_lane_objects_ground() {

if (FLAGS_show_motion_track) {
auto& lane_history_pos = lane_history_->at(k).pos;
auto& lane_z_history = z_history_.at(k);
// update lane history by projecting motion
for (auto& p : lane_history_pos) {
// for (auto& p : lane_history_pos) {
for (int i = 0; i < lane_history_pos.size(); i++) {
auto &p = lane_history_pos[i];
auto &z = lane_z_history[i];
Eigen::VectorXf point_h =
Eigen::VectorXf::Zero(motion_matrix_.cols());
point_h[0] = p[0];
point_h[1] = p[1];
point_h[2] = z;
point_h[motion_matrix_.cols()-1] = 1.0;

Eigen::Vector2f proj_h;
project_point(point_h, &proj_h, motion_matrix_);
p[0] = proj_h[0];
p[1] = proj_h[1];
z = project_point(point_h, &proj_h, motion_matrix_);
p = proj_h;
}
AINFO << "lane_history_pos.size(): " << lane_history_pos.size();
// add new point
for (auto p = lane_objects_->at(k).pos.begin();
p != lane_objects_->at(k).pos.end(); ++p) {
auto point_poly = *p;
point_poly[1] = GetPolyValue(a, b, c, d, point_poly[0]);

lane_history_pos.push_back(point_poly);
lane_z_history.push_back(0);
if (lane_history_pos.size() > lane_history_buffer_size_) {
lane_history_pos.erase(lane_history_pos.begin());
lane_z_history.erase(lane_z_history.begin());
}
}

glColor3f(1.0f, 0.0f, 0.0f); // red
// glColor3f(1.0f, 0.0f, 0.0f); // red
glLineWidth(1);
glBegin(GL_LINE_STRIP);
for (auto p : lane_history_pos) {
for (int i = 0; i < lane_history_pos.size(); i++) {
auto &p = lane_history_pos[i];
auto &z = lane_z_history[i];
// glVertex2f(p[0], p[1]);
drawHollowCircle(p[0], p[1], 0.2);
drawHollowCircle(p[0], p[1], 0.2, z);
// AINFO << "("<<p[0] << ", "<< p[1] << "), ";
}
glEnd();
glFlush();
Expand Down Expand Up @@ -1855,27 +1871,30 @@ void drawHollowCircle(GLfloat x, GLfloat y, GLfloat radius) {
}
*/

void GLFWFusionViewer::drawHollowCircle(GLfloat x, GLfloat y, GLfloat radius) {
void GLFWFusionViewer::drawHollowCircle(GLfloat x,
GLfloat y, GLfloat radius, GLfloat z) {
// number of triangles used to draw circle
GLfloat lineAmount = 100.0f;

GLfloat twicePi = 2.0f * My_PI;

glBegin(GL_LINE_LOOP);
for (GLfloat i = 0.0f; i <= lineAmount; i++) {
glVertex2f(x + (radius * cos(i * twicePi / lineAmount)),
y + (radius * sin(i * twicePi / lineAmount)));
glVertex3f(x + (radius * cos(i * twicePi / lineAmount)),
y + (radius * sin(i * twicePi / lineAmount)),
z);
}
glEnd();
}

void GLFWFusionViewer::project_point(const Eigen::VectorXf &in,
float GLFWFusionViewer::project_point(const Eigen::VectorXf &in,
Eigen::Vector2f *out,
const MotionType &motion_matrix) {
CHECK(in.rows() == motion_matrix.cols());
CHECK_GT(in.rows(), 2);
Eigen::VectorXf proj = motion_matrix * in;
*out << proj[0], proj[1];
return proj[2];
}

void GLFWFusionViewer::draw_car_trajectory(FrameContent* content) {
Expand All @@ -1890,8 +1909,9 @@ void GLFWFusionViewer::draw_car_trajectory(FrameContent* content) {
// Eigen::Matrix3f tmp = motion_buffer[i].motion;
// point = tmp * center;
Eigen::Vector2f point;
project_point(center, &point, motion_buffer[i].motion);
drawHollowCircle(point(0), point(1), 0.2);
float z = project_point(center, &point, motion_buffer[i].motion);
drawHollowCircle(point(0), point(1), 0.2, z*10);
// AINFO << "Z value is: "<< z;
glFlush();
}
}
Expand Down
7 changes: 4 additions & 3 deletions modules/perception/obstacle/camera/visualizer/glfw_fusion_viewer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ class GLFWFusionViewer {

void render();

void project_point(const Eigen::VectorXf &in, Eigen::Vector2f *out,
float project_point(const Eigen::VectorXf &in, Eigen::Vector2f *out,
const MotionType &motion_matrix);

protected:
Expand All @@ -149,7 +149,7 @@ class GLFWFusionViewer {
void draw_car_trajectory(FrameContent *content);
void draw_trajectories(FrameContent *content);

void drawHollowCircle(GLfloat x, GLfloat y, GLfloat radius);
void drawHollowCircle(GLfloat x, GLfloat y, GLfloat radius, GLfloat z = 0);

// for drawing camera 2d results
protected:
Expand Down Expand Up @@ -313,8 +313,9 @@ class GLFWFusionViewer {
float lane_map_scale_;

LaneObjectsPtr lane_history_;
std::vector<std::vector<float>> z_history_;
// std::vector<LaneObjects> Lane_history_buffer_;
const std::size_t lane_history_buffer_size_ = 400;
const std::size_t lane_history_buffer_size_ = 40000;
const std::size_t object_history_size_ = 5;
MotionType motion_matrix_;
// pin-hole camera model with distortion
Expand Down
4 changes: 2 additions & 2 deletions modules/perception/obstacle/onboard/motion_service.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,8 +94,8 @@ void MotionService::OnLocalization(
vehicle_status.time_ts = 0;

} else {
vehicle_status.roll_rate = localization.pose().angular_velocity_vrf().x();
vehicle_status.pitch_rate = localization.pose().angular_velocity_vrf().y();
vehicle_status.roll_rate = localization.pose().angular_velocity_vrf().y();
vehicle_status.pitch_rate = -localization.pose().angular_velocity_vrf().x();
vehicle_status.yaw_rate = localization.pose().angular_velocity_vrf().z();
timestamp_diff = localization.measurement_time() - pre_timestamp_;
vehicle_status.time_d = timestamp_diff;
Expand Down

0 comments on commit 8a75dba

Please sign in to comment.