diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index 72380def295597160f00e01e5c2c3d336c45ac3e..cc75f37f9d2a6e6180c69c2582459f5ecfbc54f0 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -88,7 +88,7 @@ class PublisherGraph: public Publisher
         // Options
         std::string map_frame_id_;
         bool        viz_overlapped_factors_, viz_inactive_factors_;
-        double      viz_scale_, text_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_;
+        double      viz_scale_, text_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_, frame_vel_scale_;
         std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_, factor_other_color_;
 
         // auxiliar variables
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 913a86317de13a1cfa6dd4e27061eb0ac126e365..026b2046bacac42ee1be24c5dacd057906a18527 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -49,6 +49,7 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
     // frames
     frame_width_            = getParamWithDefault<double>   (_server, prefix_ + "/frame_width", 0.1);
     frame_length_           = getParamWithDefault<double>   (_server, prefix_ + "/frame_length", 1);
+    frame_vel_scale_        = getParamWithDefault<double>   (_server, prefix_ + "/frame_vel_scale", 0.1);
     color = getParamWithDefault<Eigen::Vector4d>(_server,
                                                  prefix_ + "/frame_color",
                                                  (Eigen::Vector4d() << 1, 0.8, 0, 1).finished());
@@ -675,9 +676,9 @@ void PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm,
             if (frm->getO()->getSize() > 1)
             {
                 Eigen::Vector3d v_local = Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())).conjugate() * frm->getV()->getState();
-                frm_marker.points.back().x = v_local(0);
-                frm_marker.points.back().y = v_local(1);
-                frm_marker.points.back().z = v_local(2);
+                frm_marker.points.back().x = v_local(0) * frame_vel_scale_;
+                frm_marker.points.back().y = v_local(1) * frame_vel_scale_;
+                frm_marker.points.back().z = v_local(2) * frame_vel_scale_;
             }
             // 2d
             else