diff --git a/src/visual_gps_alg_node.cpp b/src/visual_gps_alg_node.cpp
index d41a0d2745abd2c9d51ee8947dbd14ba6cb20a31..90fd14b026795f2c57afc57cdaacdb558f6ca86a 100644
--- a/src/visual_gps_alg_node.cpp
+++ b/src/visual_gps_alg_node.cpp
@@ -382,7 +382,7 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std:
   for(uint i=0; i<map_points.size(); i++)
   {
     visualization_msgs::Marker marker;
-    marker.header.frame_id = this->config_.base_frame;
+    marker.header.frame_id = this->config_.map_frame;
     marker.ns = "map";
     marker.id = i;
     marker.header.stamp = ros::Time::now();
@@ -391,9 +391,9 @@ void VisualGpsAlgNode::publish_markers(std::vector<cv::Point3f> map_points, std:
     marker.scale.x = 0.6;
     marker.scale.y = 0.6;
     marker.scale.z = 0.1;  
-    marker.pose.position.x = img_points[i].x;
-    marker.pose.position.y = img_points[i].y;
-    marker.pose.position.z = img_points[i].z;
+    marker.pose.position.x = map_points[i].x;
+    marker.pose.position.y = map_points[i].y;
+    marker.pose.position.z = map_points[i].z;
     marker.pose.orientation.x = 0.0;
     marker.pose.orientation.y = 0.0;
     marker.pose.orientation.z = 0.0;