#912 fix

atom_core/src/atom_core/geometry.py

@@ -98,6 +98,40 @@ def rodriguesToMatrix(r):
     return matrix[0]
 
 
+def matrixToTranslationRotation(matrix):
+
+    R = matrix[0:3, 0:3]
+
+    tvec = np.zeros((3, 1), dtype=float)
+    tvec[0, 0] = matrix[0, 3]
+    tvec[1, 0] = matrix[1, 3]
+    tvec[2, 0] = matrix[2, 3]
+
+    return tvec, R
+
+
+def matrixToTranslationRodrigues(matrix):
+
+    rvec = matrixToRodrigues(matrix[0:3, 0:3])
+
+    tvec = np.zeros((1, 3), dtype=float)
+    tvec[0, 0] = matrix[0, 3]
+    tvec[0, 1] = matrix[1, 3]
+    tvec[0, 2] = matrix[2, 3]
+
+    return tvec, rvec
+
+
+def translationRotationToTransform(translation, rotation):
+    T = np.zeros((4, 4), dtype=float)
+    T[0:3, 0:3] = rotation
+    T[0, 3] = translation[0, 0]
+    T[1, 3] = translation[1, 0]
+    T[2, 3] = translation[2, 0]
+    T[3, 3] = 1
+    return T
+
+
 def traslationRodriguesToTransform(translation, rodrigues):
     R = rodriguesToMatrix(rodrigues)
     T = np.zeros((4, 4), dtype=float)

atom_evaluation/scripts/other_calibrations/test_cv_hand_eye.py

@@ -0,0 +1,17 @@
+import cv2
+import numpy as np
+
+eye_coords = np.array([[0.0, 0.0, 0.0], [0.0, 1.0, 0.0],
+                      [1.0, 1.0, 0.0], [1.0, 0.0, 0.0]])
+hand_coords = np.array([[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [
+                       1.0, 1.0, 0.0], [1.0, 0.0, 0.0]])
+# It is the rotation matrix between the hand and eye
+R_target2cam = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [
+                        0.0, 0.0, 1.0], [0.0, 0.0, 0.0]])
+# It is the translation between hand and eye
+
+x_target2cam = np.array([0.0, 0.0, 0.0, 0.0])
+# It is code for 3*4 matrix transformation
+X, _ = cv2.calibrateHandEye(hand_coords, eye_coords,
+                            R_target2cam, t_target2cam)
+print(X)

atom_examples/rihbot/rihbot_calibration/calibration/config.yml

@@ -35,7 +35,7 @@
 description_file: "package://rihbot_description/urdf/robot.urdf.xacro"
 
 # The calibration framework requires a bagfile to extract the necessary data for the calibration.
-bag_file: "$ROS_BAGS/rihbot/bag_diogo.bag"
+bag_file: "$ROS_BAGS/rihbot/train_test_opencv.bag"
 
 # You must define a frame of reference for the optimization process.
 # It must exist in the transformation chains of all the sensors which are being calibrated.

atom_examples/rihbot/rihbot_calibration/launch/collect_data.launch

@@ -25,7 +25,7 @@
 @arg marker_size   The size of the interaction marker that is used to trigger a data save.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/rihbot/bag_diogo.bag
+    default: $(env ROS_BAGS)/rihbot/train_test_opencv.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 -->
@@ -43,7 +43,7 @@
     <arg name="rviz_file" default="$(find rihbot_calibration)/rviz/collect_data.rviz"/>
     <arg name="description_file" default="$(find rihbot_calibration)/urdf/initial_estimate.urdf.xacro"/>
     <!-- arguments to be passed onto playbag.launch  -->
-    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/bag_diogo.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/train_test_opencv.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="ssl" default="lambda sensor_name: False"/>

atom_examples/rihbot/rihbot_calibration/launch/playbag.launch

@@ -22,15 +22,15 @@
     Sets up image decompressors if needed, reads the urdf robot description.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/rihbot/bag_diogo.bag
+    default: $(env ROS_BAGS)/rihbot/train_test_opencv.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 @arg use_atom_bag_file  Uses the bag file to complement the transformations generated by the urdf/joint_states/robot_state_publisher. Used in cases like softbot. default: False
 -->
 
 <launch>
 
-    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/bag_diogo.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/train_test_opencv.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="optimized" default="false"/>

atom_examples/rihbot/rihbot_calibration/launch/set_initial_estimate.launch

@@ -22,7 +22,7 @@
     Rviz interactive markers are used to set the pose of the sensors.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/rihbot/bag_diogo.bag
+    default: $(env ROS_BAGS)/rihbot/train_test_opencv.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 @arg marker_size  Size of the markers. default: 0.5
@@ -36,7 +36,7 @@
     <arg name="output_file" default="$(find rihbot_calibration)/urdf/initial_estimate.urdf.xacro"/>
     <arg name="rviz_file" default="$(find rihbot_calibration)/rviz/set_initial_estimate.rviz"/>
     <!-- arguments to be passed onto playbag.launch  -->
-    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/bag_diogo.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/rihbot/train_test_opencv.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="marker_size" default="0.5"/>