Skip to content
Snippets Groups Projects
Commit 1f38e90f authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Fixed markers: map_points coordinates and frame

parent 89b8dcc4
No related branches found
No related tags found
No related merge requests found
...@@ -382,7 +382,7 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std: ...@@ -382,7 +382,7 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std:
for(uint i=0; i<map_points.size(); i++) for(uint i=0; i<map_points.size(); i++)
{ {
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.header.frame_id = this->config_.base_frame; marker.header.frame_id = this->config_.map_frame;
marker.ns = "map"; marker.ns = "map";
marker.id = i; marker.id = i;
marker.header.stamp = ros::Time::now(); marker.header.stamp = ros::Time::now();
...@@ -391,9 +391,9 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std: ...@@ -391,9 +391,9 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std:
marker.scale.x = 0.6; marker.scale.x = 0.6;
marker.scale.y = 0.6; marker.scale.y = 0.6;
marker.scale.z = 0.1; marker.scale.z = 0.1;
marker.pose.position.x = img_points[i].x; marker.pose.position.x = map_points[i].x;
marker.pose.position.y = img_points[i].y; marker.pose.position.y = map_points[i].y;
marker.pose.position.z = img_points[i].z; marker.pose.position.z = map_points[i].z;
marker.pose.orientation.x = 0.0; marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0; marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0; marker.pose.orientation.z = 0.0;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment