added real world poses

Author: maximiliangilles

Scripts/colored_pcl.py

@@ -116,6 +116,10 @@ def rgb_score_color(score):
         "--score_threshold",
         type=float, default=-1,
         help="Only show grasps above threshold.")
+    parser.add_argument(
+        "--visualize_pose",
+        action="store_true",
+        help="Set flag to visualize object pose.")
     args = parser.parse_args()
 
     PATH_TO_DATA = pathlib.Path(args.data_root)
@@ -339,6 +343,41 @@ def rgb_score_color(score):
 
     camera_cos = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1)
 
+    meshes_cos = []
+    if args.visualize_pose:
+        # load json
+        PATH_TO_POSE = PATH_TO_SCENE / 'scene_gt.json'
+        with open(str(PATH_TO_POSE), 'r') as f:
+            poses = json.load(f)
+        # iterate over json
+        poses_in_vpt = poses[str(VIEWPT)]
+        for idx, pose in enumerate(poses_in_vpt):
+            R_m2c = pose['cam_R_m2c']
+            t_m2c = pose['cam_t_m2c']
+            mesh_class = pose['obj_id']
+
+            def hex_to_rgb(value):
+                value = value.lstrip('#')
+                lv = len(value)
+                return tuple(int(value[i:i + lv // 3], 16)/256. for i in range(0, lv, lv // 3))
+
+            color_list_hex = ['#be254a', '#dc484c', '#ef6645', '#f88c51', '#fdb365', '#fed27f', '#feeb9d', '#fffebe',
+                          '#f0f9a7', '#d8ef9b', '#b3e0a2', '#89d0a4', '#60bba8', '#3f97b7', '#4273b3']
+            
+            color_list = [hex_to_rgb(color) for color in color_list_hex]
+
+            T_m2c = np.array([[R_m2c[0][0], R_m2c[0][1], R_m2c[0][2], t_m2c[0]/100.],
+                            [R_m2c[1][0], R_m2c[1][1], R_m2c[1][2], t_m2c[1]/100.],
+                            [R_m2c[2][0], R_m2c[2][1], R_m2c[2][2], t_m2c[2]/100.],
+                            [0, 0, 0, 1]])
+
+            mesh_cos = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05)
+
+            meshes_cos.append(mesh_cos.transform(T_m2c))
+
+    # world_cos
+    world_cos = []
+
     # world_cos
     world_cos = []
     o3d.visualization.draw_geometries([
@@ -349,6 +388,7 @@ def rgb_score_color(score):
         *suctioncup_cos,
         *parallel_contacts,
         *keypts_byhand_vis,
+        *meshes_cos,
         *keypts_com_vis])
 
     if args.save_pcl:

dataset_real/scene418/scene_gt.json

@@ -0,0 +1 @@
+{"3": [{"cam_R_m2c": [[0.9918744691017879, -0.024662383424808143, -0.12480706866140685], [0.08108289162847637, -0.6334151110442392, 0.7695523775455348], [-0.09803367905577173, -0.7734190739467376, -0.6262685796255218]], "cam_t_m2c": [1.677290735488537, 5.05378314647373, 59.02736563276792], "obj_id": 47}, {"cam_R_m2c": [[0.9032590033531617, -0.33369375194494877, 0.2697622152459245], [-0.41947948005932406, -0.5543438030898655, 0.7188462379292709], [-0.09033348587676014, -0.7624640502271557, -0.6406937126588551]], "cam_t_m2c": [5.932953233597026, 5.052715941840216, 65.1035490843005], "obj_id": 47}, {"cam_R_m2c": [[0.992656926644787, -0.11176672427185713, -0.04626473094786572], [-0.03944151949541635, -0.6606231252912161, 0.749680900697341], [-0.11435292966965266, -0.7423511875628814, -0.660180370656355]], "cam_t_m2c": [0.5102389874459484, -0.0746912053696694, 64.77221055497094], "obj_id": 47}, {"cam_R_m2c": [[-0.13130505497695968, 0.7393188273969873, -0.6604291438101944], [0.9886982283639868, 0.049041392856134056, -0.14167129214021937], [-0.0723518884856893, -0.6715672812481998, -0.7374025976286497]], "cam_t_m2c": [-1.5931140927477327, 7.237390393147694, 65.75627714666443], "obj_id": 47}, {"cam_R_m2c": [[0.7956150597635958, 0.003078990267733755, 0.6057946817992887], [-0.6055582414041721, 0.03243634672117455, 0.7951396730630859], [-0.01720153902583526, -0.9994690606668704, 0.0276713538669096]], "cam_t_m2c": [6.358159387868463, -1.8616598702948577, 65.40608458105724], "obj_id": 60}, {"cam_R_m2c": [[-0.2942344367420089, -0.03412664019733795, -0.9551237975592264], [0.9512093831363163, 0.0866639960649682, -0.2961250769851814], [0.0928805989889129, -0.9956529135270492, 0.006962048302637779]], "cam_t_m2c": [-8.637046061690874, 5.124408270784697, 64.00820586669244], "obj_id": 60}, {"cam_R_m2c": [[-0.07402192237262398, 0.3437794708361188, 0.9361284262535247], [-0.10224400744505573, -0.936373219392988, 0.33578468837217135], [0.992001470739755, -0.07085809364679233, 0.1044615365333384]], "cam_t_m2c": [-13.907132543335301, 3.0138123796415788, 64.05976500632715], "obj_id": 60}, {"cam_R_m2c": [[0.12948417070509363, 0.05245086560992379, 0.990193292359418], [-0.18620182833801027, 0.9821216880653827, -0.0276743374840425], [-0.9739418507592703, -0.180792412806424, 0.13693565938510777]], "cam_t_m2c": [0.8282241211793294, -9.710954216778147, 64.94318410999277], "obj_id": 62}, {"cam_R_m2c": [[-0.15538559592081724, -0.7056171871689936, -0.6913462965490361], [0.2176340451312385, 0.6581990276777406, -0.7207006745965417], [0.9635822429767759, -0.26244699490718243, 0.051291674607505194]], "cam_t_m2c": [-7.844374592637465, -6.190358037337973, 65.64918887805933], "obj_id": 62}, {"cam_R_m2c": [[-0.057431173761236964, -0.8990557281711443, -0.43405121578340955], [0.04885868194672277, -0.43677896163066493, 0.8982410410771], [-0.9971531925831861, 0.03037986700902521, 0.06901140631575811]], "cam_t_m2c": [-10.906974771507954, 4.2234176107113255, 67.50638587921198], "obj_id": 62}]}

dataset_real/scene46/scene_gt.json

@@ -0,0 +1 @@
+{"3": [{"cam_R_m2c": [[0.06708905508593307, 0.13497727993689687, -0.9885748290283989], [-0.054498854571978644, -0.9888322131083626, -0.13871095547781945], [-0.9962574634792094, 0.06318218277381049, -0.058983711666172584]], "cam_t_m2c": [-11.300192515276365, -10.017432638766953, 64.47358832804771], "obj_id": 47}, {"cam_R_m2c": [[-0.029647524062000857, 0.05020948239824748, -0.9982985686628505], [0.47927803858750684, -0.8757260983864096, -0.05827831786031415], [-0.8771622347346629, -0.4801903877445015, 0.0018988083313068033]], "cam_t_m2c": [12.516704811213454, -10.650055180353311, 64.80807088317432], "obj_id": 51}, {"cam_R_m2c": [[0.8846532031638876, -0.02104016846945624, -0.46577464662930984], [-0.4662482808991737, -0.04231725110891767, -0.8836412115882565], [-0.001118342723210601, 0.998882656556499, -0.04724603412130757]], "cam_t_m2c": [8.007137393289035, 4.196627330279022, 65.52015103012305], "obj_id": 56}]}