Fixed ctgt calculating errors incorrectly

Author: brunofavs

atom_core/src/atom_core/utilities.py

@@ -25,7 +25,7 @@
 from pynput import keyboard
 import yaml
 
-from tf.transformations import quaternion_matrix, euler_from_matrix
+from tf.transformations import quaternion_matrix, euler_from_matrix, euler_from_quaternion
 
 from atom_core.naming import generateKey
 from atom_core.system import execute, removeColorsFromText
@@ -53,6 +53,7 @@ def compareAtomTransforms(transform_1, transform_2):
         transform_2: transformation 2
     """
 
+
     # Create a 4x4 transformation for transform_1
     t1 = quaternion_matrix(transform_1['quat'])
     t1[0:3, 3] = transform_1['trans']
@@ -61,21 +62,33 @@ def compareAtomTransforms(transform_1, transform_2):
     t2 = quaternion_matrix(transform_2['quat'])
     t2[0:3, 3] = transform_2['trans']
 
-    # Method: We will use the following method. If T1 and T2 are the same, then multiplying one by the inverse of the other will produce and identity matrix, with zero translation and rotation. So we will do the multiplication and then evaluation the amount of rotation and translation in the resulting matrix.
+    v = t2[0:3, 3] - t1[0:3, 3]
+
+    # Method: We will use the following method. If T1 and T2 are the same, then multiplying one by the inverse of the other will produce and identity matrix, with zero translation and rotation. So we will do the multiplication and then evaluation of the amount of rotation and translation in the resulting matrix.
     # print('Comparing \nt1= ' + str(t1) + ' \n\nt2=' + str(t2))
 
     t_delta = np.dot(np.linalg.inv(t1), t2)
     # print('t_delta = ' + str(t_delta))
 
     rotation_delta = t_delta[0:3, 0:3]
-    roll, pitch, yaw = euler_from_matrix(rotation_delta)
-
+    # roll, pitch, yaw = euler_from_matrix(rotation_delta)
     translation_delta = t_delta[0:3, 3]
-    # print('translation_delta = ' + str(translation_delta))
+
+    euler_angles_init = euler_from_quaternion(transform_1['quat'])
+    euler_angles_final = euler_from_quaternion(transform_2['quat'])
+
+
+    deuler = np.subtract(euler_angles_final,euler_angles_init)
+    rotation_error = np.linalg.norm(deuler)
 
     # global metrics
-    translation_error = float(abs(np.average(translation_delta)))
-    rotation_error = float(np.average([abs(roll), abs(pitch), abs(yaw)]))
+    # translation_error = float(abs(np.average(translation_delta)))
+    translation_error = np.linalg.norm(translation_delta)
+    # rotation_error = np.linalg.norm([roll, pitch, yaw])
+
+
+    # print('translation error = ' + str(translation_error))
+    # print('rotation error = ' + str(rotation_error))
 
     return translation_error, rotation_error
 
@@ -187,7 +200,7 @@ def printComparisonToGroundTruth(
     if dataset['calibration_config']['additional_tfs'] is not None:
         for additional_tf_key, additional_tf in dataset['calibration_config']['additional_tfs'].items():
 
-            transform_key = generateKey(additional_tf["parent_link"], sensor["child_link"])
+            transform_key = generateKey(additional_tf["parent_link"], additional_tf["child_link"])
             row = [transform_key, Fore.LIGHTCYAN_EX + additional_tf_key + Style.RESET_ALL]
 
 

atom_examples/softbot/softbot_calibration/urdf/optimized.urdf.xacro

@@ -373,7 +373,7 @@
     <child link="lidar3d_plate_link"/>
   </joint>
   <joint name="lidar3d_base_mount_joint" type="fixed">
-    <origin xyz="-0.0015142375046177249 -0.0022052142273190152 -0.000283564068925264" rpy="-0.007757658250765604 0.0009838709879697281 -0.014847674947914611"/>
+    <origin xyz="0.03716082344930228 -0.27220696054487664 0.061596273221005735" rpy="-0.05298232658801819 -0.03810670333805793 -0.10759100238230218"/>
     <parent link="lidar3d_plate_link"/>
     <child link="lidar3d_base_link"/>
   </joint>
@@ -493,7 +493,7 @@
     <selfCollide>false</selfCollide>
   </gazebo>
   <joint name="front_left_camera_rgb_joint" type="fixed">
-    <origin xyz="-4.399872653456163e-16 -0.04499999999999956 3.1045954087761856e-16" rpy="-4.440829884921592e-16 4.4301780635402403e-16 4.438903032319087e-16"/>
+    <origin xyz="-4.4406205425347153e-16 -0.045000000000000435 -3.3859777840638286e-16" rpy="-2.6170137566262974e-16 -1.890031503105939e-16 -4.440892059486435e-16"/>
     <parent link="front_left_camera_link"/>
     <child link="front_left_camera_rgb_frame"/>
   </joint>
@@ -586,7 +586,7 @@
     <selfCollide>false</selfCollide>
   </gazebo>
   <joint name="front_right_camera_rgb_joint" type="fixed">
-    <origin xyz="0.002030911322794242 -0.03938584552044518 -1.2175099096372358e-05" rpy="-0.005222638857532736 0.001007139992505166 -0.0037596367537089353"/>
+    <origin xyz="-0.6754329952328235 0.01920420891565159 -0.37662191151245966" rpy="0.1461866602192413 -0.12509500646354754 -0.1929261388257878"/>
     <parent link="front_right_camera_link"/>
     <child link="front_right_camera_rgb_frame"/>
   </joint>

atom_examples/softbot/softbot_calibration/urdf/optimized_w_pattern.urdf.xacro

@@ -373,7 +373,7 @@
     <child link="lidar3d_plate_link"/>
   </joint>
   <joint name="lidar3d_base_mount_joint" type="fixed">
-    <origin xyz="-0.0015142375046177249 -0.0022052142273190152 -0.000283564068925264" rpy="-0.007757658250765604 0.0009838709879697281 -0.014847674947914611"/>
+    <origin xyz="0.03716082344930228 -0.27220696054487664 0.061596273221005735" rpy="-0.05298232658801819 -0.03810670333805793 -0.10759100238230218"/>
     <parent link="lidar3d_plate_link"/>
     <child link="lidar3d_base_link"/>
   </joint>
@@ -493,7 +493,7 @@
     <selfCollide>false</selfCollide>
   </gazebo>
   <joint name="front_left_camera_rgb_joint" type="fixed">
-    <origin xyz="-4.399872653456163e-16 -0.04499999999999956 3.1045954087761856e-16" rpy="-4.440829884921592e-16 4.4301780635402403e-16 4.438903032319087e-16"/>
+    <origin xyz="-4.4406205425347153e-16 -0.045000000000000435 -3.3859777840638286e-16" rpy="-2.6170137566262974e-16 -1.890031503105939e-16 -4.440892059486435e-16"/>
     <parent link="front_left_camera_link"/>
     <child link="front_left_camera_rgb_frame"/>
   </joint>
@@ -586,7 +586,7 @@
     <selfCollide>false</selfCollide>
   </gazebo>
   <joint name="front_right_camera_rgb_joint" type="fixed">
-    <origin xyz="0.002030911322794242 -0.03938584552044518 -1.2175099096372358e-05" rpy="-0.005222638857532736 0.001007139992505166 -0.0037596367537089353"/>
+    <origin xyz="-0.6754329952328235 0.01920420891565159 -0.37662191151245966" rpy="0.1461866602192413 -0.12509500646354754 -0.1929261388257878"/>
     <parent link="front_right_camera_link"/>
     <child link="front_right_camera_rgb_frame"/>
   </joint>
@@ -655,7 +655,7 @@
     </visual>
   </link>
   <joint name="world-pattern_link" type="fixed">
-    <origin xyz="1.273180549764564 0.5275071795462576 0.9180241714021727" rpy="-1.57238029953394 -0.7458089302748174 -1.6092354412755343"/>
+    <origin xyz="1.1763285302817843 0.6949119593244414 0.7637724865844582" rpy="-1.4378439431950503 -0.421493318623612 -1.8393927091040134"/>
     <parent link="world"/>
     <child link="pattern_link"/>
   </joint>