diff --git a/LeGO-LOAM/CMakeLists.txt b/LeGO-LOAM/CMakeLists.txt
index efebe5b3..bd8e577b 100755
--- a/LeGO-LOAM/CMakeLists.txt
+++ b/LeGO-LOAM/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(lego_loam)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3")
 
 find_package(catkin REQUIRED COMPONENTS
   tf
@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(GTSAM REQUIRED QUIET)
 find_package(PCL REQUIRED QUIET)
 find_package(OpenCV REQUIRED QUIET)
+find_package(Boost REQUIRED COMPONENTS timer thread serialization chrono)
 
 catkin_package(
   INCLUDE_DIRS include
@@ -57,4 +58,4 @@ add_executable(mapOptmization src/mapOptmization.cpp)
 target_link_libraries(mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
 
 add_executable(transformFusion src/transformFusion.cpp)
-target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
\ No newline at end of file
+target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
diff --git a/LeGO-LOAM/include/utility.h b/LeGO-LOAM/include/utility.h
index 0c13827f..b2c74864 100755
--- a/LeGO-LOAM/include/utility.h
+++ b/LeGO-LOAM/include/utility.h
@@ -10,7 +10,8 @@
 
 #include "cloud_msgs/cloud_info.h"
 
-#include <opencv/cv.h>
+//#include <pcl/kdtree/kdtree_flann.h>
+#include <opencv2/opencv.hpp>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -50,14 +51,14 @@ using namespace std;
 
 typedef pcl::PointXYZI  PointType;
 
-extern const string pointCloudTopic = "/velodyne_points";
-extern const string imuTopic = "/imu/data";
+extern const string pointCloudTopic = "/mid/points";
+extern const string imuTopic = "/imu/imu_and_mag";
 
 // Save pcd
 extern const string fileDirectory = "/tmp/";
 
 // Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
-extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used
+extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
 
 // VLP-16
 extern const int N_SCAN = 16;
@@ -101,7 +102,7 @@ extern const int groundScanInd = 7;
 // extern const float ang_bottom = 16.6+0.1;
 // extern const int groundScanInd = 15;
 
-extern const bool loopClosureEnableFlag = false;
+extern const bool loopClosureEnableFlag = true;
 extern const double mappingProcessInterval = 0.3;
 
 extern const float scanPeriod = 0.1;
diff --git a/LeGO-LOAM/launch/run.launch b/LeGO-LOAM/launch/run.launch
index 65a5a543..190c9324 100755
--- a/LeGO-LOAM/launch/run.launch
+++ b/LeGO-LOAM/launch/run.launch
@@ -1,17 +1,22 @@
 <launch>
     
     <!--- Sim Time -->
-    <param name="/use_sim_time" value="true" />
+    <!-- <param name="/use_sim_time" value="true" /> -->
+    <arg name="imu_topic" default="/imu/data"/>
+    <arg name="point_cloud_topic" default="/mid/points"/>
 
     <!--- Run Rviz-->
     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" />
 
     <!--- TF -->
-    <node pkg="tf" type="static_transform_publisher" name="camera_init_to_map"  args="0 0 0 1.570795   0        1.570795 /map    /camera_init 10" />
-    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        /camera /base_link   10" />
+    <node pkg="tf" type="static_transform_publisher" name="camera_init_to_odom"  args="0 0 0 1.570795   0        1.570795 odom    camera_init 10" />
+    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        camera base_link   10" />
 
     <!--- LeGO-LOAM -->    
-    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen"/>
+    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen">
+        <remap from="/mid/points" to="$(arg point_cloud_topic)"/>
+        <remap from="/imu/data" to="$(arg imu_topic)"/>
+    </node>
     <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
     <node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
     <node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/>
diff --git a/LeGO-LOAM/launch/test.rviz b/LeGO-LOAM/launch/test.rviz
index 721f8b2c..2bbb6bd1 100644
--- a/LeGO-LOAM/launch/test.rviz
+++ b/LeGO-LOAM/launch/test.rviz
@@ -4,27 +4,30 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /TF1/Frames1
-      Splitter Ratio: 0.703999996
-    Tree Height: 707
+      Splitter Ratio: 0.7039999961853027
+    Tree Height: 272
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
-    Expanded: ~
+    Expanded:
+      - /2D Nav Goal1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Edge Features (green)
+    SyncSource: Surface Features (pink)
   - Class: rviz/Help
     Name: Help
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -32,9 +35,12 @@ Visualization Manager:
   Displays:
     - Class: rviz/TF
       Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
+      Marker Alpha: 1
       Marker Scale: 10
       Name: TF
       Show Arrows: true
@@ -44,25 +50,29 @@ Visualization Manager:
         {}
       Update Interval: 0
       Value: false
-    - Class: rviz/Axes
-      Enabled: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: false
       Length: 2
       Name: map
-      Radius: 0.300000012
+      Radius: 0.30000001192092896
       Reference Frame: map
-      Value: true
-    - Class: rviz/Axes
+      Show Trail: false
+      Value: false
+    - Alpha: 1
+      Class: rviz/Axes
       Enabled: true
       Length: 1
       Name: base_link
       Radius: 0.5
       Reference Frame: base_link
+      Show Trail: false
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 17.0059433
-        Min Value: -1.04788589
+        Max Value: 17.005943298339844
+        Min Value: -1.0478858947753906
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -73,15 +83,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 183
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Velodyne
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /full_cloud_projected
       Unreliable: false
@@ -91,8 +99,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.4566002
-        Min Value: -3.58307004
+        Max Value: 20.456600189208984
+        Min Value: -3.5830700397491455
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -103,15 +111,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 96
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Surface (yellow)
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 8
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /laser_cloud_flat
       Unreliable: false
@@ -121,8 +127,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.4566002
-        Min Value: -3.58307004
+        Max Value: 20.456600189208984
+        Min Value: -3.5830700397491455
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -133,15 +139,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 96
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Edge Sharp (blue)
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 8
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /laser_cloud_sharp
       Unreliable: false
@@ -151,8 +155,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.4566002
-        Min Value: -3.58307004
+        Max Value: 20.456600189208984
+        Min Value: -3.5830700397491455
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -160,29 +164,27 @@ Visualization Manager:
       Color: 0; 255; 0
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 96
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Edge Features (green)
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 5
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /laser_cloud_less_sharp
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.4566002
-        Min Value: -3.58307004
+        Max Value: 20.456600189208984
+        Min Value: -3.5830700397491455
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -193,15 +195,13 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 96
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Surface Features (pink)
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /laser_cloud_less_flat
       Unreliable: false
@@ -211,8 +211,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10.7254
-        Min Value: -6.34819984
+        Max Value: 10.7253999710083
+        Min Value: -6.348199844360352
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -223,15 +223,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15.1604996
       Min Color: 0; 0; 0
-      Min Intensity: 8.00349998
       Name: Outlier Cloud
       Position Transformer: XYZ
       Queue Size: 2
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /outlier_cloud
       Unreliable: false
@@ -253,15 +251,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15.1787996
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Segmentation Full
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /segmented_cloud
       Unreliable: false
@@ -283,15 +279,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 241
       Min Color: 0; 0; 0
-      Min Intensity: 1
       Name: Segmentation Pure
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 5
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /segmented_cloud_pure
       Unreliable: false
@@ -301,8 +295,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 18.9591999
-        Min Value: 0.0404983014
+        Max Value: 18.959199905395508
+        Min Value: 0.04049830138683319
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -313,15 +307,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Ground Cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /ground_cloud
       Unreliable: false
@@ -331,8 +323,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 4.57089996
-        Min Value: 0.300000012
+        Max Value: 4.570899963378906
+        Min Value: 0.30000001192092896
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -343,15 +335,13 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 42
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Trajectory
       Position Transformer: XYZ
       Queue Size: 1
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.300000012
+      Size (m): 0.30000001192092896
       Style: Flat Squares
       Topic: /key_pose_origin
       Unreliable: false
@@ -373,15 +363,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: ICP cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 5
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /corrected_cloud
       Unreliable: false
@@ -403,15 +391,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: History Key Frames
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /history_cloud
       Unreliable: false
@@ -421,8 +407,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 23.0833607
-        Min Value: -3.98265076
+        Max Value: 23.08336067199707
+        Min Value: -3.9826507568359375
         Value: true
       Axis: Y
       Channel Name: intensity
@@ -433,15 +419,13 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Map Cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 1
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /laser_cloud_surround
       Unreliable: false
@@ -451,8 +435,8 @@ Visualization Manager:
     - Alpha: 0.5
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 52.9331703
-        Min Value: -7.77158022
+        Max Value: 8.569266319274902
+        Min Value: -1.1881691217422485
         Value: true
       Axis: Y
       Channel Name: intensity
@@ -463,15 +447,13 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Map Cloud (stack)
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /registered_cloud
       Unreliable: false
@@ -481,8 +463,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.2114735
-        Min Value: -2.70486069
+        Max Value: 20.21147346496582
+        Min Value: -2.7048606872558594
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -493,21 +475,46 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 15
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: Surround Cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 2
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /recent_cloud
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.30000001192092896
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz/PoseArray
+      Color: 0; 255; 127
+      Enabled: true
+      Head Length: 0.5
+      Head Radius: 0.5
+      Name: PoseArray
+      Queue Size: 10
+      Shaft Length: 0.699999988079071
+      Shaft Radius: 0.20000000298023224
+      Shape: Arrow (3D)
+      Topic: /waypoints
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
@@ -520,39 +527,35 @@ Visualization Manager:
       Hide Inactive Objects: true
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
+    - Class: rviz/SetGoal
+      Topic: goal
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
-      Distance: 86.4874191
+      Angle: 3.8550047874450684
+      Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.829797029
+      Near Clip Distance: 0.009999999776482582
+      Scale: 28.60154151916504
       Target Frame: base_link
-      Value: Orbit (rviz)
-      Yaw: 0.395514786
+      X: 1.4938398599624634
+      Y: -1.225363850593567
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 1028
+    collapsed: true
+  Height: 1371
   Help:
     collapsed: false
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000179000003b5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e000003b5000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c0070000000032c000000ba0000007900ffffff000000010000010f000003befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003be000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000039fc0100000003fb0000001600520061006e0067006500200049006d00610067006500000000000000073f0000000000000000fb0000000a0049006d0061006700650000000000000006100000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006500000000000000073e0000038300fffffffb0000000800540069006d0065010000000000000450000000000000000000000600000003b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000017500000505fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000505000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c0070000000032c000000ba0000006e00ffffff000000010000010f00000505fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000505000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000039fc0100000003fb0000001600520061006e0067006500200049006d00610067006500000000000000073f0000000000000000fb0000000a0049006d0061006700650000000000000006100000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006500000000000000073e0000035c00fffffffb0000000800540069006d00650100000000000004500000000000000000000009c60000050500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -561,6 +564,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 637
-  Y: 227
+  Width: 2502
+  X: 58
+  Y: 32
diff --git a/LeGO-LOAM/src/featureAssociation.cpp b/LeGO-LOAM/src/featureAssociation.cpp
index a13447f8..82e33715 100755
--- a/LeGO-LOAM/src/featureAssociation.cpp
+++ b/LeGO-LOAM/src/featureAssociation.cpp
@@ -302,11 +302,11 @@ class FeatureAssociation{
         kdtreeCornerLast.reset(new pcl::KdTreeFLANN<PointType>());
         kdtreeSurfLast.reset(new pcl::KdTreeFLANN<PointType>());
 
-        laserOdometry.header.frame_id = "/camera_init";
-        laserOdometry.child_frame_id = "/laser_odom";
+        laserOdometry.header.frame_id = "camera_init";
+        laserOdometry.child_frame_id = "laser_odom";
 
-        laserOdometryTrans.frame_id_ = "/camera_init";
-        laserOdometryTrans.child_frame_id_ = "/laser_odom";
+        laserOdometryTrans.frame_id_ = "camera_init";
+        laserOdometryTrans.child_frame_id_ = "laser_odom";
         
         isDegenerate = false;
         matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
@@ -790,28 +790,28 @@ class FeatureAssociation{
 	    if (pubCornerPointsSharp.getNumSubscribers() != 0){
 	        pcl::toROSMsg(*cornerPointsSharp, laserCloudOutMsg);
 	        laserCloudOutMsg.header.stamp = cloudHeader.stamp;
-	        laserCloudOutMsg.header.frame_id = "/camera";
+	        laserCloudOutMsg.header.frame_id = "camera";
 	        pubCornerPointsSharp.publish(laserCloudOutMsg);
 	    }
 
 	    if (pubCornerPointsLessSharp.getNumSubscribers() != 0){
 	        pcl::toROSMsg(*cornerPointsLessSharp, laserCloudOutMsg);
 	        laserCloudOutMsg.header.stamp = cloudHeader.stamp;
-	        laserCloudOutMsg.header.frame_id = "/camera";
+	        laserCloudOutMsg.header.frame_id = "camera";
 	        pubCornerPointsLessSharp.publish(laserCloudOutMsg);
 	    }
 
 	    if (pubSurfPointsFlat.getNumSubscribers() != 0){
 	        pcl::toROSMsg(*surfPointsFlat, laserCloudOutMsg);
 	        laserCloudOutMsg.header.stamp = cloudHeader.stamp;
-	        laserCloudOutMsg.header.frame_id = "/camera";
+	        laserCloudOutMsg.header.frame_id = "camera";
 	        pubSurfPointsFlat.publish(laserCloudOutMsg);
 	    }
 
 	    if (pubSurfPointsLessFlat.getNumSubscribers() != 0){
 	        pcl::toROSMsg(*surfPointsLessFlat, laserCloudOutMsg);
 	        laserCloudOutMsg.header.stamp = cloudHeader.stamp;
-	        laserCloudOutMsg.header.frame_id = "/camera";
+	        laserCloudOutMsg.header.frame_id = "camera";
 	        pubSurfPointsLessFlat.publish(laserCloudOutMsg);
 	    }
     }
@@ -1621,13 +1621,13 @@ class FeatureAssociation{
         sensor_msgs::PointCloud2 laserCloudCornerLast2;
         pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
         laserCloudCornerLast2.header.stamp = cloudHeader.stamp;
-        laserCloudCornerLast2.header.frame_id = "/camera";
+        laserCloudCornerLast2.header.frame_id = "camera";
         pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
 
         sensor_msgs::PointCloud2 laserCloudSurfLast2;
         pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
         laserCloudSurfLast2.header.stamp = cloudHeader.stamp;
-        laserCloudSurfLast2.header.frame_id = "/camera";
+        laserCloudSurfLast2.header.frame_id = "camera";
         pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
 
         transformSum[0] += imuPitchStart;
@@ -1797,19 +1797,19 @@ class FeatureAssociation{
             sensor_msgs::PointCloud2 outlierCloudLast2;
             pcl::toROSMsg(*outlierCloud, outlierCloudLast2);
             outlierCloudLast2.header.stamp = cloudHeader.stamp;
-            outlierCloudLast2.header.frame_id = "/camera";
+            outlierCloudLast2.header.frame_id = "camera";
             pubOutlierCloudLast.publish(outlierCloudLast2);
 
             sensor_msgs::PointCloud2 laserCloudCornerLast2;
             pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
             laserCloudCornerLast2.header.stamp = cloudHeader.stamp;
-            laserCloudCornerLast2.header.frame_id = "/camera";
+            laserCloudCornerLast2.header.frame_id = "camera";
             pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
 
             sensor_msgs::PointCloud2 laserCloudSurfLast2;
             pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
             laserCloudSurfLast2.header.stamp = cloudHeader.stamp;
-            laserCloudSurfLast2.header.frame_id = "/camera";
+            laserCloudSurfLast2.header.frame_id = "camera";
             pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
         }
     }
diff --git a/LeGO-LOAM/src/mapOptmization.cpp b/LeGO-LOAM/src/mapOptmization.cpp
index 56088327..be809872 100755
--- a/LeGO-LOAM/src/mapOptmization.cpp
+++ b/LeGO-LOAM/src/mapOptmization.cpp
@@ -256,11 +256,11 @@ class mapOptimization{
         downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); // for global map visualization
         downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); // for global map visualization
 
-        odomAftMapped.header.frame_id = "/camera_init";
-        odomAftMapped.child_frame_id = "/aft_mapped";
+        odomAftMapped.header.frame_id = "camera_init";
+        odomAftMapped.child_frame_id = "aft_mapped";
 
-        aftMappedTrans.frame_id_ = "/camera_init";
-        aftMappedTrans.child_frame_id_ = "/aft_mapped";
+        aftMappedTrans.frame_id_ = "camera_init";
+        aftMappedTrans.child_frame_id_ = "aft_mapped";
 
         allocateMemory();
     }
@@ -695,7 +695,7 @@ class mapOptimization{
             sensor_msgs::PointCloud2 cloudMsgTemp;
             pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp);
             cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-            cloudMsgTemp.header.frame_id = "/camera_init";
+            cloudMsgTemp.header.frame_id = "camera_init";
             pubKeyPoses.publish(cloudMsgTemp);
         }
 
@@ -703,7 +703,7 @@ class mapOptimization{
             sensor_msgs::PointCloud2 cloudMsgTemp;
             pcl::toROSMsg(*laserCloudSurfFromMapDS, cloudMsgTemp);
             cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-            cloudMsgTemp.header.frame_id = "/camera_init";
+            cloudMsgTemp.header.frame_id = "camera_init";
             pubRecentKeyFrames.publish(cloudMsgTemp);
         }
 
@@ -716,7 +716,7 @@ class mapOptimization{
             sensor_msgs::PointCloud2 cloudMsgTemp;
             pcl::toROSMsg(*cloudOut, cloudMsgTemp);
             cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-            cloudMsgTemp.header.frame_id = "/camera_init";
+            cloudMsgTemp.header.frame_id = "camera_init";
             pubRegisteredCloud.publish(cloudMsgTemp);
         } 
     }
@@ -790,7 +790,7 @@ class mapOptimization{
         sensor_msgs::PointCloud2 cloudMsgTemp;
         pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp);
         cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-        cloudMsgTemp.header.frame_id = "/camera_init";
+        cloudMsgTemp.header.frame_id = "camera_init";
         pubLaserCloudSurround.publish(cloudMsgTemp);  
 
         globalMapKeyPoses->clear();
@@ -864,7 +864,7 @@ class mapOptimization{
             sensor_msgs::PointCloud2 cloudMsgTemp;
             pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);
             cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-            cloudMsgTemp.header.frame_id = "/camera_init";
+            cloudMsgTemp.header.frame_id = "camera_init";
             pubHistoryKeyFrames.publish(cloudMsgTemp);
         }
 
@@ -910,7 +910,7 @@ class mapOptimization{
             sensor_msgs::PointCloud2 cloudMsgTemp;
             pcl::toROSMsg(*closed_cloud, cloudMsgTemp);
             cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
-            cloudMsgTemp.header.frame_id = "/camera_init";
+            cloudMsgTemp.header.frame_id = "camera_init";
             pubIcpKeyFrames.publish(cloudMsgTemp);
         }   
         /*
diff --git a/LeGO-LOAM/src/transformFusion.cpp b/LeGO-LOAM/src/transformFusion.cpp
index 73fb3abc..383a4cc3 100755
--- a/LeGO-LOAM/src/transformFusion.cpp
+++ b/LeGO-LOAM/src/transformFusion.cpp
@@ -69,17 +69,17 @@ class TransformFusion{
         subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this);
         subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this);
 
-        laserOdometry2.header.frame_id = "/camera_init";
-        laserOdometry2.child_frame_id = "/camera";
+        laserOdometry2.header.frame_id = "camera_init";
+        laserOdometry2.child_frame_id = "camera";
 
-        laserOdometryTrans2.frame_id_ = "/camera_init";
-        laserOdometryTrans2.child_frame_id_ = "/camera";
+        laserOdometryTrans2.frame_id_ = "camera_init";
+        laserOdometryTrans2.child_frame_id_ = "camera";
 
-        map_2_camera_init_Trans.frame_id_ = "/map";
-        map_2_camera_init_Trans.child_frame_id_ = "/camera_init";
+        map_2_camera_init_Trans.frame_id_ = "map";
+        map_2_camera_init_Trans.child_frame_id_ = "camera_init";
 
-        camera_2_base_link_Trans.frame_id_ = "/camera";
-        camera_2_base_link_Trans.child_frame_id_ = "/base_link";
+        camera_2_base_link_Trans.frame_id_ = "camera";
+        camera_2_base_link_Trans.child_frame_id_ = "base_link";
 
         for (int i = 0; i < 6; ++i)
         {
@@ -251,4 +251,4 @@ int main(int argc, char** argv)
     ros::spin();
 
     return 0;
-}
+}
\ No newline at end of file