diff --git a/iplanner/data_generation.py b/iplanner/data_generation.py
index a853750..de423f0 100644
--- a/iplanner/data_generation.py
+++ b/iplanner/data_generation.py
@@ -20,7 +20,7 @@
         parameters = json.load(json_file)
     
     folder_name = parameters.get('folder_name', "CollectedData")
-    folder_path = os.path.join(*[os.getcwd(), "data"])
+    folder_path = os.path.join(*[root_folder, "data"])
     ids_path = os.path.join(folder_path, "collect_list.txt")
     
     if not folder_name == "":
@@ -33,7 +33,7 @@
     print("Env List: ", env_list)
 
     outfolder_name = parameters.get('outfolder_name', "TrainingData")
-    output_folder = os.path.join(*[os.getcwd(), "data", outfolder_name])
+    output_folder = os.path.join(*[root_folder, "data", outfolder_name])
 
     image_type = parameters.get('image_type', "depth")
     voxel_size = parameters.get('voxel_size', 0.05)
diff --git a/iplanner/esdf_mapping.py b/iplanner/esdf_mapping.py
index 275fdf4..1ee3580 100644
--- a/iplanner/esdf_mapping.py
+++ b/iplanner/esdf_mapping.py
@@ -78,8 +78,11 @@ def read_intrinsic(intrinsic_path):
         with open(intrinsic_path) as f:
             lines = f.readlines()
             elems = np.fromstring(lines[0][1:-1], dtype=float, sep=', ')
-        P = np.array(elems).reshape(3, 4)
-        K = np.concatenate((P, np.array([0.0, 0.0, 0.0, 1.0]).reshape(1,-1)), axis=0)
+        if len(elems) == 12:
+            P = np.array(elems).reshape(3, 4)
+            K = np.concatenate((P, np.array([0.0, 0.0, 0.0, 1.0]).reshape(1,-1)), axis=0)
+        else:
+            K = np.array(elems).reshape(4, 4)
         return K
 
     @staticmethod
@@ -87,8 +90,8 @@ def read_extrinsic(extrinsic_path):
         with open(extrinsic_path) as f:
             lines = f.readlines()
             elems = np.fromstring(lines[0][1:-1], dtype=float, sep=', ')
-        CT = np.array(elems[:3])
-        CR = R.from_quat(elems[3:])
+        CR = R.from_quat(elems[:4])
+        CT = np.array(elems[4:])
         return CR, CT
 
     @staticmethod
diff --git a/src/data_collect_node.py b/src/data_collect_node.py
index 53e7527..8061a60 100644
--- a/src/data_collect_node.py
+++ b/src/data_collect_node.py
@@ -215,7 +215,7 @@ def __colorInfoCallback(self, color_info, args):
         return
 
     def __writeExtrinstic(self, pos, ori, path, name):
-        extric_list = [pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3]]
+        extric_list = [ori[0], ori[1], ori[2], ori[3], pos[0], pos[1], pos[2]]
         open(path, 'w').close()  # clear txt file
         fc = open(path, 'w')
         fc.writelines(str(extric_list))