Merge remote-tracking branch 'upstream/master'

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)

iplanner/esdf_mapping.py

@@ -78,17 +78,20 @@ 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
     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

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))