diff --git a/CMakeLists.txt b/CMakeLists.txt
index 88fbd34..a42b47e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,10 +25,12 @@ find_package(catkin REQUIRED cmake_modules
   camera_info_manager
   tf
   eigen_conversions
+  tf_conversions
 )
 set(CMAKE_AUTOMOC ON)
 
 find_package(Eigen REQUIRED)
+#find_package(PCL /usr/lib/x86_64-linux-gnu/cmake)
 
 #set (CMAKE_PREFIX_PATH /opt/Qt5.6.1/5.6/gcc_64/lib/cmake)
 
@@ -68,6 +70,7 @@ catkin_package(
     pcl_conversions  
     pcl_msgs
     eigen_conversions
+    tf_conversions
 #    irp_sen_msgs
     camera_info_manager
     tf
diff --git a/package.xml b/package.xml
index 0800011..247142f 100644
--- a/package.xml
+++ b/package.xml
@@ -54,6 +54,8 @@
   <build_depend>camera_info_manager</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>eigen_conversions</build_depend>
+  <build_depend>tf_conversions</build_depend>
+
 
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
@@ -72,6 +74,7 @@
   <run_depend>camera_info_manager</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>eigen_conversions</run_depend>
+  <run_depend>tf_conversions</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/rviz/vis.rviz b/rviz/vis.rviz
new file mode 100644
index 0000000..5d708c1
--- /dev/null
+++ b/rviz/vis.rviz
@@ -0,0 +1,259 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Odometry1
+        - /Odometry1/Shape1
+        - /PointCloud22
+        - /PointCloud22/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 659
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    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: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        ouster:
+          Value: true
+        radar_polar:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          base_link:
+            ouster:
+              {}
+            radar_polar:
+              {}
+      Update Interval: 0
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 10000
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.10000000149011612
+        Head Radius: 0.20000000298023224
+        Shaft Length: 0.10000000149011612
+        Shaft Radius: 0.10000000149011612
+        Value: Arrow
+      Topic: /gt
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: false
+      Image Topic: /Navtech/Polar
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+    - Alpha: 0.20000000298023224
+      Autocompute Intensity Bounds: false
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 1
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1000
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /os1_points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 136
+      Min Color: 0; 0; 0
+      Min Intensity: 60
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /Navtech/Filtered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 8.321887969970703
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 4.821981430053711
+        Y: -1.9836641550064087
+        Z: 4.061916828155518
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: -0.01459960825741291
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 6.136727809906006
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 958
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000031efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002af000000ae0000001600ffffff000000010000010f0000031efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000031e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 27
diff --git a/src/ROSThread.cpp b/src/ROSThread.cpp
index 7ec002d..50514f6 100644
--- a/src/ROSThread.cpp
+++ b/src/ROSThread.cpp
@@ -36,6 +36,9 @@ ROSThread::ROSThread(QObject *parent, QMutex *th_mutex)
   stamp_show_count_ = 0;
   imu_data_version_ = 0;
   prev_clock_stamp_ = 0;
+  Tbase2lidar = vectorToAffine3d({1.7042, -0.021, 1.8047, 0.0001*M_PI/180.0, 0.0003*M_PI/180.0, 179.6654*M_PI/180.0 });
+
+  Tbase2radar = vectorToAffine3d({1.50, -0.04, 1.97, 0.0000*M_PI/180.0, 0.0000*M_PI/180.0, 0.9*M_PI/180.0 });
 }
 
 
@@ -61,6 +64,7 @@ ROSThread::~ROSThread()
   radarpolar_thread_.cv_.notify_all(); // giseop
   if(radarpolar_thread_.thread_.joinable()) radarpolar_thread_.thread_.join();
 
+
 }
 
 
@@ -115,6 +119,9 @@ ROSThread::Ready()
   radarpolar_thread_.cv_.notify_all();
   if(radarpolar_thread_.thread_.joinable()) radarpolar_thread_.thread_.join();
 
+
+
+
   //check path is right or not
   ifstream f((data_folder_path_+"/sensor_data/data_stamp.csv").c_str());
   if(!f.good()){
@@ -276,6 +283,7 @@ ROSThread::Ready()
   ouster_thread_.active_ = true;
   radarpolar_thread_.active_ = true;
 
+
   data_stamp_thread_.thread_ = std::thread(&ROSThread::DataStampThread,this);
   gps_thread_.thread_ = std::thread(&ROSThread::GpsThread,this);
   imu_thread_.thread_ = std::thread(&ROSThread::ImuThread,this);
@@ -699,16 +707,63 @@ ROSThread::ResetProcessStamp(int position)
   }
 }
 
+geometry_msgs::TransformStamped ROSThread::EigToGeomStamped(const Eigen::Affine3d& T, const ros::Time& t, const std::string& parent, const std::string& child){
+  geometry_msgs::TransformStamped Tstamped;
+  tf::Transform tf_T;
+  tf::poseEigenToTF(T, tf_T);
+  tf::transformStampedTFToMsg(tf::StampedTransform(tf_T, t, parent, child), Tstamped);
+  return Tstamped;
+}
 
-
-void ROSThread::SaveRosbag()
+void
+ROSThread::SaveOuster(rosbag::Bag &bag)
 {
-  rosbag::Bag bag;
-  const std::string bag_path = data_folder_path_+"/output.bag";
-  bag.open(data_folder_path_+"/output.bag", rosbag::bagmode::Write);
-  cout<<"Storing bag to: "<<bag_path<<endl;
 
+  int count = 0;
+  //Store Ouster data
+  GetDirList(data_folder_path_ + "/sensor_data/Ouster", ouster_file_list_);
+
+  for(auto && data_filending : ouster_file_list_){
+    std::string data = data_filending.substr(0, data_filending.size()-4);
+
+    pcl::PointCloud<PointXYZIRT> cloud;
+    cloud.clear();
+    sensor_msgs::PointCloud2 publish_cloud;
+
+    string current_file_name = data_folder_path_ + "/sensor_data/Ouster" +"/"+ data + ".bin";
+    std::cout << data << std::endl;
+    uint64 u_data = stol(data);
+    std::cout << "data: " << u_data << std::endl;
+    std::cout <<  current_file_name << std::endl;
+    ifstream file;
+    file.open(current_file_name, ios::in|ios::binary);
+    int k = 0;
+    while(!file.eof())
+    {
+      PointXYZIRT point;
+      file.read(reinterpret_cast<char *>(&point.x), sizeof(float));
+      file.read(reinterpret_cast<char *>(&point.y), sizeof(float));
+      file.read(reinterpret_cast<char *>(&point.z), sizeof(float));
+      file.read(reinterpret_cast<char *>(&point.intensity), sizeof(float));
+      point.ring = (k%64) + 1 ;
+      k = k+1 ;
+      cloud.points.push_back (point);
+    }
+    file.close();
+
+    pcl::toROSMsg(cloud, publish_cloud);
+    publish_cloud.header.stamp.fromNSec(u_data);
+    publish_cloud.header.frame_id = "ouster";
+    bag.write("/os1_points", publish_cloud.header.stamp, publish_cloud);
+    cout<<"Written: "<<++count<<"/"<<ouster_file_list_.size() <<" /os1_points"<<endl;
 
+  }
+}
+void
+ROSThread::SaveRadar(rosbag::Bag &bag){
+
+
+  //Store Navtech
   GetDirList(data_folder_path_ + "/sensor_data/radar/polar", radarpolar_file_list_);
 
   int current_img_index = 0;
@@ -738,8 +793,12 @@ void ROSThread::SaveRosbag()
     auto msg = radarpolar_out_msg.toImageMsg();
     bag.write("/Navtech/Polar", msg->header.stamp, *msg);
   }
-  ////////////////////// OPEN GROUND TRUTH FILE /////////////////////
+}
+
+void
+ROSThread::SaveGT(rosbag::Bag &bag){
 
+  ////////////////////// OPEN GROUND TRUTH FILE /////////////////////
   const std::string gt_csv_path = data_folder_path_+ std::string("/global_pose.csv");
   fstream fin;
   fin.open(gt_csv_path, ios::in);
@@ -762,8 +821,10 @@ void ROSThread::SaveRosbag()
         row.push_back(str);
       if(row.size()!=13)
         break;
-      int64_t stamp_int;
-      std::istringstream ( row[0] ) >> stamp_int;
+
+      uint64 stamp_int = stol(row[0]);
+      //std::istringstream ( row[0] ) >> stamp_int;
+      //std::cout << stamp_int << std::endl;
       for(int i=0;i<3;i++){
         for(int j=0;j<4;j++){
           double d = boost::lexical_cast<double> (row[1+(4*i)+j]);
@@ -771,14 +832,61 @@ void ROSThread::SaveRosbag()
         }
       }
 
+      ros::Time t;
+      t.fromNSec(stamp_int);
       Eigen::Affine3d Tgt(T);
+      static Eigen::Affine3d Tinit = Tgt;
+      Tgt = Tinit.inverse()*Tgt; // Start at Zero \in SE(3)
       //std::cout<<Tgt.matrix()<<std::endl;
-      tf::poseEigenToMsg(Tgt,Tgt_msg.pose.pose);
-      Tgt_msg.header.stamp.fromNSec(stamp_int);
+
+
+      //Store odometry messages
+      Tgt_msg.header.stamp = t;
+      tf::poseEigenToMsg(Tgt, Tgt_msg.pose.pose);
       bag.write("/gt", Tgt_msg.header.stamp, Tgt_msg);
-      //cout<<"Written: "<<count++<<" /gt nav_msgs/odometry poses to /gt"<<endl;
+
+      const Eigen::Affine3d Tgt_lidar = Tgt*Tbase2lidar;
+      tf::poseEigenToMsg(Tgt_lidar, Tgt_msg.pose.pose);
+      bag.write("/gt_lidar", Tgt_msg.header.stamp, Tgt_msg);
+
+      const Eigen::Affine3d Tgt_radar = Tgt*Tbase2radar;
+      tf::poseEigenToMsg(Tgt_radar, Tgt_msg.pose.pose);
+      bag.write("/gt_radar", Tgt_msg.header.stamp, Tgt_msg);
+
+      // Store TF's
+      geometry_msgs::TransformStamped tf_world_base = EigToGeomStamped(Tgt, t, "/world", "/base_link");
+      geometry_msgs::TransformStamped tf_base_lidar = EigToGeomStamped(Tbase2lidar, t, "/base_link", "/ouster");
+      geometry_msgs::TransformStamped tf_base_radar = EigToGeomStamped(Tbase2radar, t, "/base_link", "/radar_polar");
+      tf::tfMessage tfmsg;
+      tfmsg.transforms = {tf_world_base, tf_base_lidar, tf_base_radar};
+      bag.write("/tf", t, tfmsg);
+
     }
   }
+}
+void
+ROSThread::SaveRosbag()
+{
+  // General initialization
+  rosbag::Bag bag;
+  const std::string bag_path = data_folder_path_+"/output.bag";
+  bag.open(data_folder_path_+"/output.bag", rosbag::bagmode::Write);
+  cout<<"Storing bag to: "<<bag_path<<endl;
+
+  SaveOuster(bag);
+  SaveRadar(bag);
+  SaveGT(bag);
+
+
+  //End
   cout<<"rosbag stored at: "<<bag_path<<endl;
   bag.close();
 }
+
+Eigen::Affine3d ROSThread::vectorToAffine3d(const std::vector<double>& v) {
+
+  return Eigen::Translation<double, 3>(v[0], v[1], v[2]) *
+      Eigen::AngleAxis<double>(v[3], Eigen::Vector3d::UnitX()) *
+      Eigen::AngleAxis<double>(v[4], Eigen::Vector3d::UnitY()) *
+      Eigen::AngleAxis<double>(v[5], Eigen::Vector3d::UnitZ());
+}
diff --git a/src/ROSThread.h b/src/ROSThread.h
index 3a0c849..a7718fa 100644
--- a/src/ROSThread.h
+++ b/src/ROSThread.h
@@ -83,7 +83,10 @@
 #include "tf2/LinearMath/Transform.h"
 #include "csetjmp"
 #include "eigen_conversions/eigen_msg.h"
-
+#include "tf/transform_broadcaster.h"
+#include "tf/tfMessage.h"
+#include "eigen_conversions/eigen_msg.h"
+#include "tf_conversions/tf_eigen.h"
 using namespace std;
 using namespace cv;
 
@@ -116,7 +119,15 @@ class ROSThread : public QThread
 
     int imu_data_version_;
 
+    Eigen::Affine3d Tbase2lidar, Tbase2radar;
+
+    // ---- ROSBAG RELATED METHODS ---- //
     void SaveRosbag();
+    void SaveOuster(rosbag::Bag &bag);
+    void SaveRadar(rosbag::Bag &bag);
+    void SaveGT(rosbag::Bag &bag);
+    geometry_msgs::TransformStamped EigToGeomStamped(const Eigen::Affine3d& T, const ros::Time& t, const std::string& parent, const std::string& child);
+
     void Ready();
     void ResetProcessStamp(int position);
 
@@ -165,6 +176,8 @@ class ROSThread : public QThread
     void OusterThread(); // giseop
     void RadarpolarThread(); 
 
+    Eigen::Affine3d vectorToAffine3d(const std::vector<double>& v);
+
     void FilePlayerStart(const std_msgs::BoolConstPtr& msg);
     void FilePlayerStop(const std_msgs::BoolConstPtr& msg);