Merge pull request #5 from mpkuse/master-se

Master se

CMakeLists.txt

@@ -171,6 +171,7 @@ add_executable(
  src/ProcessedLoopCandidate.cpp
  src/DlsPnpWithRansac.cpp
  src/HypothesisManager.cpp
+ src/ImageDataManager.cpp
  ${KuseUtilsSources}
  ${CamodocalCameraModelSources}
 )

README.md

@@ -150,6 +150,7 @@ If you wish to train your own model, you may use [my learning code here](https:
 - *loopcandidate_consumer_th* : Computes the relative pose at the loopcandidates. Publishes the loopEdge.
 - *kidnap_th* : Identifies kidnap. If kidnap publishes the reset signals for vins_estimator.
 - *viz_th* : Publishes the image-pair, and more things for debugging and analysis.
+- *dm_cleanup_th* : Deallocate/Store to file images to reduce RAM consumption.
 
 
 **Nvidia TX2**: Often times for live run, you might want to run the

config/vinsfusion/mynteye/mynteye_stereo_imu_config.yaml

@@ -51,7 +51,7 @@ body_T_cam1: !!opencv-matrix
 multiple_thread: 1
 
 #feature traker paprameters
-max_cnt: 120 # max feature number in feature tracking
+max_cnt: 100 # max feature number in feature tracking
 min_dist: 30 # min distance between two features
 freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
 F_threshold: 1.1 # ransac threshold (pixel)

launch/mynteye_vinsfusion.launch

@@ -5,7 +5,7 @@
 
 
  <!-- BAG -->
- <arg name="bag_path" default="/Bulk_Data/ros_bags/mynteye/" />
+ <!-- <arg name="bag_path" default="/Bulk_Data/ros_bags/mynteye/" /> -->
 
  <!-- ### IN LAB -->
  <!-- <arg name="bag_file" default="1.bag" doc="sitting on my desk"/> -->
@@ -31,6 +31,7 @@
  <!-- <arg name="bag_file" default="/kidnap/2019-01-14-17-20-06.bag" doc="lab kidnap. easy"/> -->
  <!-- <arg name="bag_file" default="/kidnap/2019-02-14-12-00-21.bag" doc="3 kidnaps in lab. This gives the chained case for pose computation between worlds"/> -->
  <arg name="bag_file" default="/kidnap/2019-02-14-17-19-29.bag" doc="4 kidnaps in lab"/>
+ <!-- <arg name="bag_file" default="/kidnap/2019-02-14-17-19-29.bag" doc="4 kidnaps in lab"/> -->
  <!-- <arg name="bag_file" default="/kidnap/2019-02-21-17-10-30.bag" doc="0:-; 1:-; 2:merge(0), merge(1). 2nd merges with both 0 and 1."/> -->
  <!-- <arg name="bag_file" default="/kidnap/2019-02-21-17-38-58.bag" doc="0:-; 1:-; 2:merge(0), merge(1). 2nd merges with both 0 and 1."/> -->
  <!-- <arg name="bag_file" default="/kidnap/2019-03-07-14-58-00.bag" doc="2 independent runs"/> -->
@@ -59,6 +60,7 @@
 
  <node pkg="rosbag" type="play" name="rosbag" args="$(arg bag_path)/$(arg bag_file) -s 1 -d 10" output="log"/>
 
+
  <!-- END Bag -->
 
 

launch/mynteye_vinsfusion_ondrone.launch

@@ -0,0 +1,117 @@
+<launch timeout="100.0">
+
+ <!-- CONFIG FILE -->
+ <arg name="config_path" default="$(find cerebro)/config/vinsfusion/mynteye/mynteye_stereo_imu_config.yaml" />
+
+
+ <!-- BAG -->
+ <!-- <arg name="bag_path" default="/Bulk_Data/ros_bags/mynteye/" /> -->
+
+ <!-- ### IN LAB -->
+ <!-- <arg name="bag_file" default="1.bag" doc="sitting on my desk"/> -->
+ <!-- <arg name="bag_file" default="2018-11-20-17-45-55.bag" doc="single loop in lab"/> -->
+ <!-- <arg name="bag_file" default="2018-11-20-17-42-54.bag" doc="multiple loops in lab"/> -->
+ <!-- <arg name="bag_file" default="2019-01-28-16-32-23.bag" doc="many-10 loops in lab"/> -->
+ <!-- <arg name="bag_file" default="2018-11-22-12-06-47.bag" doc="drone fly area loopy"/> -->
+ <!-- <arg name="bag_file" default="2018-11-22-12-09-21.bag" doc="conference room RI"/> -->
+ <!-- <arg name="bag_file" default="2018-11-29-12-37-49.bag" doc="lab rotated loops"/> -->
+ <!-- <arg name="bag_file" default="2018-11-29-15-10-24.bag" doc="outside RI"/> -->
+ <!-- <arg name="bag_file" default="2019-01-14-18-25-18.bag" doc="lab AR test"/> -->
+
+ <!-- ### In HKUST Academic block and CYT UG -->
+ <!-- <arg name="bag_file" default="2018-11-29-15-13-44.bag" doc="CYT UG floor"/> -->
+ <!-- <arg name="bag_file" default="2018-11-29-15-22-50.bag" doc="academic concourse, no loop in this"/> -->
+ <!-- <arg name="bag_file" default="2018-11-29-15-30-39.bag" doc="near coffee shop 1"/> -->
+ <!-- <arg name="bag_file" default="2018-11-29-15-36-41.bag" doc="seng"/> -->
+ <!-- <arg name="bag_file" default="2018-12-13-16-48-46.bag" doc="seng3. walking on academic concouse CYT end, walking down the escalator and back up."/> -->
+ <!-- <arg name="bag_file" default="2018-12-13-16-56-36.bag" doc="concourse-seng4. from coffee-shop to atrium"/> -->
+
+ <!-- ### Kidnap sequences in lab -->
+ <!-- <arg name="bag_path" default="/Bulk_Data/ros_bags/mynteye/" /> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-01-14-17-20-06.bag" doc="lab kidnap. easy"/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-02-14-12-00-21.bag" doc="3 kidnaps in lab. This gives the chained case for pose computation between worlds"/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-02-14-17-19-29.bag" doc="4 kidnaps in lab"/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-02-21-17-10-30.bag" doc="0:-; 1:-; 2:merge(0), merge(1). 2nd merges with both 0 and 1."/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-02-21-17-38-58.bag" doc="0:-; 1:-; 2:merge(0), merge(1). 2nd merges with both 0 and 1."/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-03-07-14-58-00.bag" doc="2 independent runs"/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-03-14-16-12-22.bag" doc="7 kidnaps. 0:; 1:; all next merges with 1. Final merge with 0 and 3."/> -->
+ <!-- <arg name="bag_file" default="/kidnap/2019-03-14-16-36-58.bag" doc="10 kidnaps. 0; 1; merge everything with 1. finall all merge to zero. very complicated dependence structure of the walk,"/> -->
+
+
+ <!-- ### Mall -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-08-16-02-10.bag" doc=""/> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-08-16-09-41.bag" doc="good. no kidnap, but just 1 loop starting from Muji"/> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-08-16-14-19.bag" doc="1 moderate loop;kidnap;a very short seq in a portion of the loop. near marathon sports, (shoes shops)"/> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-08-16-17-05.bag" doc="corridor near shoes shop;kidnap;"/> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-07-51.bag" doc="3 kidnaps. k1:hsbc;right to muji; shoes;;k1: panash;atrium;shoes;panash" /> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-17-34.bag" doc="2 kidnaps. k1:fancl;bossino;muji;right of hsbc rotated and tilted; k2:bodyshop ; k3: fancl;bossini,shoes" /> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-40-35.bag" doc="level-1 from from PacificC to muji to hsbc. 1kidnap" /> -->
+
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-23-38.bag" doc="start from atrium near pacific-coffee;go up; casual walking on top floor 4 kidnaps" /> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-34-01.bag" doc="1 corridor on top floor. no loop no kidnap" /> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-34-57.bag" doc="1 kidnaps. both runs r independent, no interworld revisit" /> -->
+ <!-- <arg name="bag_file" default="/mall/2019-04-10-15-38-51.bag" doc="really short (abt 30 sec near broadway)" /> -->
+
+
+
+
+
+
+ <!-- <node pkg="rosbag" type="play" name="rosbag" args="$(arg bag_path)/$(arg bag_file) -s 1 -d 2" output="log"/> -->
+
+ <!-- END Bag -->
+
+
+ <!-- VINS-Fusion Estimator -->
+ <!-- unlike previous vins-mono, even the feature tracker is put inside 1 node ie. vins_node.
+ $(ubuntu) rosrun vins vins_node /app/catkin_ws/src/cerebro/config/vinsfusion/mynteye/mynteye_stereo_imu_config.yaml
+ -->
+ <node name="vins_estimator" pkg="vins" type="vins_node" args=" $(arg config_path)" output="log">
+ </node>
+
+ <group if="0">
+ <!-- VINS-Fusion loop_fusion -->
+ <!-- This is the DBOW + posegraph optimization from Qin Tong
+ $(ubuntu) rosrun loop_fusion loop_fusion_node /app/catkin_ws/src/cerebro/config/vinsfusion/euroc/euroc_stereo_imu_config.yaml
+ -->
+ <node name="loop_fusion" pkg="loop_fusion" type="loop_fusion_node" args=" $(arg config_path)" output="log">
+ </node>
+ </group>
+
+
+ <group if="1">
+ <!-- Cerebro -->
+ <node name="cerebro_node" pkg="cerebro" type="cerebro_node" output="screen">
+ <param name="config_file" type="string" value="$(arg config_path)" />
+ </node>
+
+ <!-- KERAS SERVER -->
+ <!-- $(ubuntu) rosrun cerebro whole_image_desc_compute_server.py _config_file:=/app/catk_ws/src/cerebro/config/vinsfusion/euroc/euroc_stereo_imu_config.yaml -->
+ <!-- Since I run the server on tx2 dont need to run it here -->
+ <group if="0" >
+ <node name="my_desc_server" pkg="cerebro" type="whole_image_desc_compute_server.py" output="screen">
+ <!-- <param name="kerasmodel_file" type="string" value="$(find cerebro)/scripts/keras.models/modelarch_and_weights.h5" /> -->
+ <!-- <param name="kerasmodel_file" type="string" value="/models.keras/June2019/centeredinput-m1to1-240x320x3__mobilenet-conv_pw_6_relu__K16__allpairloss/modelarch_and_weights.500.h5" /> -->
+ <param name="kerasmodel_file" type="string" value="$(find cerebro)/scripts/keras.models/June2019/centeredinput-m1to1-240x320x3__mobilenet-conv_pw_6_relu__K16__allpairloss/modelarch_and_weights.700.h5" />
+
+
+ <param name="config_file" type="string" value="$(arg config_path)" />
+ <!-- OR -->
+ <!--
+ <param name="nrows" type="string" value="480" />
+ <param name="ncols" type="string" value="752" />
+ -->
+
+ <!-- this is needed irrespective of the config_file or the nrows and ncols set manually here. -->
+ <!-- <param name="nchnls" type="string" value="1" /> -->
+ <param name="nchnls" type="string" value="3" />
+ </node>
+
+ <!-- Pose graph (kidnap aware) -->
+ <!-- $(ubuntu) rosrun solve_keyframe_pose_graph keyframe_pose_graph_slam -->
+ <node name="keyframe_pose_graph_slam_node" pkg="solve_keyframe_pose_graph" type="keyframe_pose_graph_slam" output="log" >
+ </node>
+ </group>
+ </group>
+
+</launch>

rviz/vins-fusion.rviz

@@ -51,6 +51,8 @@ Panels:
  - /Current View1
  Name: Views
  Splitter Ratio: 0.5
+Toolbars:
+ toolButtonStyle: 2
 Visualization Manager:
  Class: ""
  Displays:
@@ -289,6 +291,7 @@ Visualization Manager:
  X: 0
  Y: 0
  Z: 0
+ Pose Color: 255; 85; 255
  Pose Style: None
  Radius: 0.0299999993
  Shaft Diameter: 0.100000001
@@ -311,6 +314,7 @@ Visualization Manager:
  X: 0
  Y: 0
  Z: 0
+ Pose Color: 255; 85; 255
  Pose Style: None
  Radius: 0.0299999993
  Shaft Diameter: 0.100000001
@@ -339,6 +343,7 @@ Visualization Manager:
  Enabled: true
  Global Options:
  Background Color: 48; 48; 48
+ Default Light: true
  Fixed Frame: world
  Frame Rate: 30
  Name: root
@@ -372,6 +377,7 @@ Visualization Manager:
  Z: -9.27598476
  Focal Shape Fixed Size: false
  Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
  Name: Current View
  Near Clip Distance: 0.00999999978
  Pitch: 1.56979632
@@ -392,6 +398,7 @@ Visualization Manager:
  Z: -6.84013748
  Focal Shape Fixed Size: true
  Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
  Name: Orbit
  Near Clip Distance: 0.00999999978
  Pitch: 1.16479671