From da17901faabd643be2c1eb1082c8202b9ad89574 Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Wed, 31 Mar 2021 14:16:57 +0200
Subject: [PATCH] fix bug 238

---
 svo/include/svo/depth_filter.h                |  6 ++++--
 svo_ros/CMakeLists.txt                        |  4 ++--
 .../launch/{test_rig3.launch => test.launch}  |  6 +++---
 svo_ros/launch/usb_cam.launch                 | 21 +++++++++++++++++++
 svo_ros/param/camera_pinhole.yaml             | 18 ++++++++--------
 svo_ros/src/vo_node.cpp                       |  1 +
 6 files changed, 40 insertions(+), 16 deletions(-)
 rename svo_ros/launch/{test_rig3.launch => test.launch} (96%)
 create mode 100644 svo_ros/launch/usb_cam.launch

diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h
index 70f6df3..1612d0e 100644
--- a/svo/include/svo/depth_filter.h
+++ b/svo/include/svo/depth_filter.h
@@ -119,7 +119,8 @@ public:
   void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
 
   /// Return a reference to the seeds. This is NOT THREAD SAFE!
-  std::list<Seed>& getSeeds() { return seeds_; }
+  //std::list<Seed>& getSeeds() { return seeds_; } Bug 238
+  std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
 
   /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
   static void updateSeed(
@@ -137,7 +138,8 @@ public:
 protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
-  std::list<Seed> seeds_;
+  //std::list<Seed> seeds_;
+  std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
   boost::mutex seeds_mut_;
   bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
   boost::thread* thread_;
diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt
index f55b7fa..d0ce982 100644
--- a/svo_ros/CMakeLists.txt
+++ b/svo_ros/CMakeLists.txt
@@ -75,5 +75,5 @@ TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})
 # Create Executables                      
 ADD_EXECUTABLE(vo src/vo_node.cpp)
 TARGET_LINK_LIBRARIES(vo svo_visualizer)
-ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)
-TARGET_LINK_LIBRARIES(benchmark svo_visualizer)
+#ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)
+#TARGET_LINK_LIBRARIES(benchmark svo_visualizer)
diff --git a/svo_ros/launch/test_rig3.launch b/svo_ros/launch/test.launch
similarity index 96%
rename from svo_ros/launch/test_rig3.launch
rename to svo_ros/launch/test.launch
index e49f9e2..f83d479 100644
--- a/svo_ros/launch/test_rig3.launch
+++ b/svo_ros/launch/test.launch
@@ -4,13 +4,13 @@
 
     <!-- Camera topic to subscribe to -->
     <param name="cam_topic" value="/camera/image_raw" type="str" />
-    
+
     <!-- Camera calibration file -->
     <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
 
     <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
     <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
-    
+
     <!-- Initial camera orientation, make it point downwards -->
     <param name="init_rx" value="3.14" />
     <param name="init_ry" value="0.00" />
@@ -18,4 +18,4 @@
 
   </node>
 
-</launch>
+</launch>
\ No newline at end of file
diff --git a/svo_ros/launch/usb_cam.launch b/svo_ros/launch/usb_cam.launch
new file mode 100644
index 0000000..ad34684
--- /dev/null
+++ b/svo_ros/launch/usb_cam.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/CoreNode/grey_img" type="str" />
+    
+    <!-- Camera calibration file -->
+    <rosparam file="$(find svo_ros)/param/camera_pinhole.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+    
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="0.00" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>
diff --git a/svo_ros/param/camera_pinhole.yaml b/svo_ros/param/camera_pinhole.yaml
index 4898bcd..506a856 100644
--- a/svo_ros/param/camera_pinhole.yaml
+++ b/svo_ros/param/camera_pinhole.yaml
@@ -1,11 +1,11 @@
 cam_model: Pinhole
-cam_width: 752
+cam_width: 640
 cam_height: 480
-cam_fx: 414.536145
-cam_fy: 414.284429
-cam_cx: 348.804988
-cam_cy: 240.076451
-cam_d0: -0.283076
-cam_d1: 0.066674
-cam_d2: 0.000896
-cam_d3: 0.000778
\ No newline at end of file
+cam_fx: 789.860721
+cam_fy: 790.097440
+cam_cx: 305.323524
+cam_cy: 260.464072
+cam_d0: 0.027470
+cam_d1: -0.051518
+cam_d2: 0.004375
+cam_d3: -0.004028
\ No newline at end of file
diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp
index c9fbb84..3417b4d 100644
--- a/svo_ros/src/vo_node.cpp
+++ b/svo_ros/src/vo_node.cpp
@@ -116,6 +116,7 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
     usleep(100000);
 }
 
+
 void VoNode::processUserActions()
 {
   char input = remote_input_.c_str()[0];