diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake
index 7109877..a2c9f00 100644
--- a/ov_msckf/cmake/ROS1.cmake
+++ b/ov_msckf/cmake/ROS1.cmake
@@ -131,7 +131,14 @@ if (catkin_FOUND AND ENABLE_ROS)
     install(TARGETS image_render_combine
             ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
             LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-            RUNTIME DESTINATION $[CATKIN_PACKAGE_BIN_DESTINATION})
+            RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+    add_executable(render_cubes src/render_cubes.cpp)
+    target_link_libraries(render_cubes ov_msckf_lib ${thirdparty_libraries})
+    install(TARGETS render_cubes
+            ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+            LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+            RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 endif ()
 
diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch
index 751978a..cbfef6c 100644
--- a/ov_msckf/launch/subscribe.launch
+++ b/ov_msckf/launch/subscribe.launch
@@ -29,6 +29,7 @@
     <!-- MASTER NODE! -->
 <!--    <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
 	<node name="image_render_combine" pkg="ov_msckf" type="image_render_combine" output="screen" clear_params="true" required="true"> </node>
+	<node name="render_cubes" pkg="ov_msckf" type="render_cubes" output="screen" clear_params="true" required="true"></node>
     <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
 
         <!-- master configuration object -->
diff --git a/ov_msckf/src/render_cubes.cpp b/ov_msckf/src/render_cubes.cpp
new file mode 100644
index 0000000..9386577
--- /dev/null
+++ b/ov_msckf/src/render_cubes.cpp
@@ -0,0 +1,87 @@
+//
+// Created by Podmogilnyi Ivan on 17.08.2022.
+//
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/CameraInfo.h>
+
+float yCubesVisPos = -2.0;
+float skew = 5.0;
+std::vector<std::vector<double>> cubeVisPoints = {
+        {71.215, 32.694, yCubesVisPos+2.5},
+//            {81.668, 26.227, yCubesVisPos},
+        {104.061, 36.691, yCubesVisPos+2.0},
+//            {90.726, 32.825, yCubesVisPos},
+        {109.541, 60.415, yCubesVisPos+1.5},
+//            {149.264, 69.490, yCubesVisPos},
+        {183.605, 91.474, yCubesVisPos+0.5},
+//            {198.294, 97.113, yCubesVisPos},
+        {251.391, 154.108, yCubesVisPos-1.0},
+//            {310.030, 190.208, yCubesVisPos},
+        {330.554, 183.244, yCubesVisPos-2.0},
+        {746.086, 450.563, yCubesVisPos-2.0},
+//            {792.608, 527.140, yCubesVisPos},
+        {883.669, 517.444, yCubesVisPos-2.0},
+        {957.811, 652.638, yCubesVisPos-2.0},
+        {1086.045, 669.317, yCubesVisPos-2.0},
+        {1125.310, 728.214, yCubesVisPos-2.0},
+        {731.996, 425.021, yCubesVisPos-2.0},
+
+        {1186.527, 700.189, yCubesVisPos},
+//            {1205.432, 681.457, yCubesVisPos},
+        {1184.375, 735.604, yCubesVisPos},
+//            {1232.697, 715.647, yCubesVisPos},
+        {1244.767, 784.257, yCubesVisPos}
+};
+
+ros::Publisher pub_cubes;
+ros::Subscriber sub_imu;
+visualization_msgs::Marker Marker;
+
+void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
+    ROS_INFO_STREAM("received imu");
+    pub_cubes.publish(Marker);
+
+}
+
+int main(int argc, char* argv[]){
+    ros::init(argc, argv, "render_cubes");
+
+    ros::NodeHandle nh;
+
+    // the idea: set the subscriber for IMU, and every time the IMU measurement comes, publish
+    // the cubes because the images are also publish when the IMU measurement arrives.
+    sub_imu = nh.subscribe("/imu/data_raw", 1000, &callback_imu);
+    pub_cubes = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
+
+    Marker.header.frame_id = "global";
+    Marker.header.stamp = ros::Time();
+    Marker.ns = "my_namespace";
+    Marker.id = 0;
+    Marker.type = visualization_msgs::Marker::CUBE_LIST;
+    Marker.action = visualization_msgs::Marker::ADD;
+    Marker.pose.orientation.x = 0.0;
+    Marker.pose.orientation.y = 0.0;
+    Marker.pose.orientation.z = 0.0;
+    Marker.pose.orientation.w = 1.0;
+    Marker.scale.x = 6.8f;
+    Marker.scale.y = 6.8f;
+    Marker.scale.z = 6.8f;
+    Marker.color.a = 1.0; // Don't forget to set the alpha!
+    Marker.color.r = 0.0;
+    Marker.color.g = 1.0;
+    Marker.color.b = 0.0;
+//only if using a MESH_RESOURCE marker type:
+    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+
+    for (auto & cubeVisPoint : cubeVisPoints){
+        geometry_msgs::Point tempPoint;
+        tempPoint.x = cubeVisPoint[0];
+        tempPoint.y = cubeVisPoint[1];
+        tempPoint.z = cubeVisPoint[2];
+        Marker.points.push_back(tempPoint);
+    }
+
+    ros::spin();
+}
diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp
index 09b094d..d1258d2 100644
--- a/ov_msckf/src/ros/ROS1Visualizer.cpp
+++ b/ov_msckf/src/ros/ROS1Visualizer.cpp
@@ -93,7 +93,8 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
   }
   fT_aligned << "ts mT_aligned" << std::endl;
 
-  subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this);
+  //// UNCOMMENT THIS TO RESTORE THE T_align_callback
+//  subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this);
 
     std::ifstream fileTraj;
     fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt");
@@ -135,30 +136,37 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
 
   pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", 2);
   pub_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/cam0_info", 2);
-  vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
+//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
+//  vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
 //  pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2);
 //  pub_camera_info_trackhist = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/trackhist", 2);
 //    camPub = nh->advertise<image_transport::CameraPublisher>("ov_msckf/camera_info", 2);
 //camPub = it.advertiseCamera("/ov_msckf/image_view", 2);
-    Marker.header.frame_id = "global";
-    Marker.header.stamp = ros::Time();
-    Marker.ns = "my_namespace";
-    Marker.id = 0;
-    Marker.type = visualization_msgs::Marker::CUBE_LIST;
-    Marker.action = visualization_msgs::Marker::ADD;
-    Marker.pose.orientation.x = 0.0;
-    Marker.pose.orientation.y = 0.0;
-    Marker.pose.orientation.z = 0.0;
-    Marker.pose.orientation.w = 1.0;
-    Marker.scale.x = 6.8f;
-    Marker.scale.y = 6.8f;
-    Marker.scale.z = 6.8f;
-    Marker.color.a = 1.0; // Don't forget to set the alpha!
-    Marker.color.r = 0.0;
-    Marker.color.g = 1.0;
-    Marker.color.b = 0.0;
-    //only if using a MESH_RESOURCE marker type:
-    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+
+//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
+//
+//    Marker.header.frame_id = "global";
+//    Marker.header.stamp = ros::Time();
+//    Marker.ns = "my_namespace";
+//    Marker.id = 0;
+//    Marker.type = visualization_msgs::Marker::CUBE_LIST;
+//    Marker.action = visualization_msgs::Marker::ADD;
+//    Marker.pose.orientation.x = 0.0;
+//    Marker.pose.orientation.y = 0.0;
+//    Marker.pose.orientation.z = 0.0;
+//    Marker.pose.orientation.w = 1.0;
+//    Marker.scale.x = 6.8f;
+//    Marker.scale.y = 6.8f;
+//    Marker.scale.z = 6.8f;
+//    Marker.color.a = 1.0; // Don't forget to set the alpha!
+//    Marker.color.r = 0.0;
+//    Marker.color.g = 1.0;
+//    Marker.color.b = 0.0;
+//    //only if using a MESH_RESOURCE marker type:
+//    Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+
+
+
 
 //    cubeVisPoints = {{10.0, 10.0, 0.02}, {10, -10, 0.02},
 //                                                    {50, 10, 0.02}, {50, 30, 0.02},
@@ -168,36 +176,39 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
 //                                                    {240, 90, 0.02}, {80, 110, 0.02},
 //                                                    {280, 110, 0.02}, {100, 130, 0.02},
 //                                                    {320, 130, 0.02}, {320, 150, 0.02}};
-    float yCubesVisPos = -6.0;
-    float skew = 5.0;
-    cubeVisPoints = {
-            {71.215, 32.694, yCubesVisPos+3},
-//            {81.668, 26.227, yCubesVisPos},
-            {84.455, 41.731, yCubesVisPos+3},
-//            {90.726, 32.825, yCubesVisPos},
-            {109.541, 60.415, yCubesVisPos+3},
-//            {149.264, 69.490, yCubesVisPos},
-            {150.645, 89.474, yCubesVisPos+2},
-//            {198.294, 97.113, yCubesVisPos},
-            {251.391, 154.108, yCubesVisPos},
-//            {310.030, 190.208, yCubesVisPos},
-            {330.554, 183.244, yCubesVisPos},
-            {746.086, 450.563, yCubesVisPos},
-//            {792.608, 527.140, yCubesVisPos},
-            {883.669, 517.444, yCubesVisPos},
-            {957.811, 652.638, yCubesVisPos},
-            {1086.045, 669.317, yCubesVisPos},
-            {1125.310, 728.214, yCubesVisPos},
-            {731.996, 425.021, yCubesVisPos},
 
-            {1186.527, 700.189, yCubesVisPos},
-//            {1205.432, 681.457, yCubesVisPos},
-            {1184.375, 735.604, yCubesVisPos},
-//            {1232.697, 715.647, yCubesVisPos},
-            {1244.767, 784.257, yCubesVisPos}
-    };
-    cashedCubeVisPoints = cubeVisPoints;
-  // End of added by PI
+
+//// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
+//    float yCubesVisPos = 3.0;
+//    float skew = 5.0;
+//    cubeVisPoints = {
+//            {71.215, 32.694, yCubesVisPos+3},
+////            {81.668, 26.227, yCubesVisPos},
+//            {84.455, 41.731, yCubesVisPos+3},
+////            {90.726, 32.825, yCubesVisPos},
+//            {109.541, 60.415, yCubesVisPos+3},
+////            {149.264, 69.490, yCubesVisPos},
+//            {150.645, 89.474, yCubesVisPos+2},
+////            {198.294, 97.113, yCubesVisPos},
+//            {251.391, 154.108, yCubesVisPos},
+////            {310.030, 190.208, yCubesVisPos},
+//            {330.554, 183.244, yCubesVisPos},
+//            {746.086, 450.563, yCubesVisPos},
+////            {792.608, 527.140, yCubesVisPos},
+//            {883.669, 517.444, yCubesVisPos},
+//            {957.811, 652.638, yCubesVisPos},
+//            {1086.045, 669.317, yCubesVisPos},
+//            {1125.310, 728.214, yCubesVisPos},
+//            {731.996, 425.021, yCubesVisPos},
+//
+//            {1186.527, 700.189, yCubesVisPos},
+////            {1205.432, 681.457, yCubesVisPos},
+//            {1184.375, 735.604, yCubesVisPos},
+////            {1232.697, 715.647, yCubesVisPos},
+//            {1244.767, 784.257, yCubesVisPos}
+//    };
+//    cashedCubeVisPoints = cubeVisPoints;
+//  // End of added by PI
 
   // option `to enable publishing of global to IMU transformation
   nh->param<bool>("publish_global_to_imu_tf", publish_global2imu_tf, true);
@@ -772,14 +783,15 @@ void ROS1Visualizer::publish_images() {
     camInfoGlob.width = exl_msg->width / 2;
     pub_cam0_info.publish(camInfoGlob);
 
-    for (auto & cubeVisPoint : cubeVisPoints){
-        geometry_msgs::Point tempPoint;
-        tempPoint.x = cubeVisPoint[0];
-        tempPoint.y = cubeVisPoint[1];
-        tempPoint.z = cubeVisPoint[2];
-        Marker.points.push_back(tempPoint);
-    }
-    vis_pub.publish( Marker );
+//// UNCOMMENT THIS TO RESTORE THE CUBES PUBLISHING
+//    for (auto & cubeVisPoint : cubeVisPoints){
+//        geometry_msgs::Point tempPoint;
+//        tempPoint.x = cubeVisPoint[0];
+//        tempPoint.y = cubeVisPoint[1];
+//        tempPoint.z = cubeVisPoint[2];
+//        Marker.points.push_back(tempPoint);
+//    }
+//    vis_pub.publish( Marker );
 
 //    pub_camera_info.publish(cameraparams);
 //    pub_camera_info_trackhist.publish(cameraparams);