documentation:vision:opencv_hand_eye_calibration [robotwiki]

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== OpenCV Hand-Eye Calibration ======
Using the OpenCV to execute the Hand-Eye calibraiton is very efficent and comes with good results.
Their method implements the solutions to the **AX = XB** equation. The current implementation uses [[https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gaebfc1c9f7434196a374c382abf43439b|calibrateHandEye()]].

There are a couple different methods that can be used to solve the issue, the choices of methods can be found [[https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#gad10a5ef12ee3499a0774c7904a801b99|here]].

The functions expects the rotation matrices from **R_gripper2base** and the translation part from the homogenous matrix **t_gripper2base**, as well as the corresponding parts for the **target2cam**.

__NOTE:__ Current implementation does not allow fixed camera when doing the hand-eye calibraiton but one could put an ArUco marker in the middle of the gripper and move the gripper around the camera's view frame. This would require an entire new skill or major changes to the current implementation.

----
==== The Skill ====
Input elements:
	* **Camera** - Camera to do the calibration for.
	* **Arm** - Arm connected with the camera.
	* **Object** - Reference Point used when generating poses, also the target.
	* **Radius** - Radius of sphere when generating poses.
	* **MaxDistance** - Limits the maximum distance of the generated poses from the base of the `Arm`.
	* **MinDistance** - Minimum distance from the base for the generated poses.
	* **TheesholdPos** - How accurate the //go_to_linear()// functions needs to be before saying that it reached its destination.
	* **Number of Poses** - Number of poses for the hand-eye calibration with a minimum of 2.

Required world model relations:
	* **Arm** ''skiros:hasA'' **EELink**
	* **Camera** ''skiros:hasA'' **View Frame**

The **Arm** need the the following properties set in the world model:
	* ''skiros:LinkedToFrameId''
        * ''skiros:MotionExe''
        * ''skiros:MoveItGroup'' (when using MoveIt implementation)
        * ''skiros:MoveItReferenceFrame'' (when using MoveIt implementation)
        * ''skiros:MoveItTCPLink'' (when using MoveIt implementation)


The **EELink** (should be renamed to EndEffectorLink) is defined as:

  scalable:ArmEELink-888 a scalable:ArmEELink,
        owl:NamedIndividual ;
    rdfs:label "Arm End Effector Link" ;
    skiros:BaseFrameId "cora:Robot-1"^^xsd:string ;
    skiros:ContainerState "Empty"^^xsd:string ;
    skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
    skiros:FrameId "ur5e_ee_link"^^xsd:string ;
    skiros:FrontAxis "x"^^xsd:string;
    skiros:LinkedToFrameId "ur5e_ee_link"^^xsd:string ;
    skiros:Orientation...
    skiros:Position...
    skiros:PublishTf true;
    skiros:Template "scalable:arm_end_effector"^^xsd:string ;
    skiros:TfTimeStamp "282.412"^^xsd:float ;
    skiros:at skiros:BlockGraspPose-1 ;
    skiros:hasA scalable:WsgGripper-3 ;
    skiros:contain skiros:DepthCamera-48,
        skiros:TransformationPose-887 ;.

An **Object** to be observed using ArUco Marker should be defined as:


    skiros:BigBox-143 a skiros:Product,
        owl:NamedIndividual ;
    rdfs:label "BlockWithMarker42" ;
    skiros:BaseFrameId "skiros:DepthCamera-48"^^xsd:string ;
    skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
    skiros:FrameId "skiros:BigBox-143"^^xsd:string ;
    skiros:OrientationW "1.0"^^xsd:float ;
    skiros:OrientationX "0.0"^^xsd:float ;
    skiros:OrientationY "0.0"^^xsd:float ;
    skiros:OrientationZ "0.0"^^xsd:float ;
    skiros:PositionX "0.0"^^xsd:float ;
    skiros:PositionY "0.0"^^xsd:float ;
    skiros:PositionZ "0.0"^^xsd:float ;
    skiros:SizeX "0.325"^^xsd:float ;
    skiros:SizeY "0.324"^^xsd:float ;
    skiros:SizeZ "0.236"^^xsd:float ;
    skiros:PublishTf true ;
    skiros:Template "skiros:Product"^^xsd:string ;
    skiros:TfTimeStamp "1639494837.17"^^xsd:float ;
    skiros:at skiros:Location-3 ;
    skiros:hasA scalable:Marker-422-7 .

and its corresponding **Marker**:

    scalable:Marker-422-7 a scalable:Marker,
	owl:NamedIndividual ;
	rdfs:label "ArucoMarker0"^^xsd:string ;
	skiros:BaseFrameId "skiros:BigBox-143"^^xsd:string ;
    skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
    skiros:FrameId "skiros:Marker-422-7"^^xsd:string ;
	skiros:OrientationW "0.0"^^xsd:float ;
	skiros:OrientationX "0.7071068"^^xsd:float ;
	skiros:OrientationY "0.7071068"^^xsd:float ;
	skiros:OrientationZ "0.0"^^xsd:float ;
	skiros:PositionX "-0.087"^^xsd:float ;
	skiros:PositionY "-0.087"^^xsd:float ;
	skiros:PositionZ "0.0"^^xsd:float ;
    skiros:PublishTf true ;
	skiros:Size "0.174"^^xsd:float ;
	skiros:Value "422"^^xsd:int ;
	scalable:ArUcoSize "7x7"^^xsd:string .

A **Camera** ''skiros:hasA'' **CalibrationParameters** which also needs to be modified when getting the data using the //aau_spatial_reasonor.py//, these are defined as:
  scalable:CalibrationParameters-1 a     scalable:CalibrationParameters,
        owl:NamedIndividual ;
    rdfs:label "real_sense_calibration" ;
    scalable:FocalLengthX "607.91"^^xsd:float ;
    scalable:FocalLengthY "608.40"^^xsd:float ;
    scalable:PixelCenterX "435.50"^^xsd:float ;
    scalable:PixelCenterY "225.05"^^xsd:float ;
    scalable:Distortionk1 "0.1203"^^xsd:float ;
    scalable:Distortionk2 "-0.3019"^^xsd:float ;
    scalable:Distortionp1 "0.0002"^^xsd:float ;
    scalable:Distortionp2 "-0.0044"^^xsd:float ;
    scalable:Distortionk3 "0.1847"^^xsd:float ; .
    
Required skills needed by the **SkillBase**
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/ceb6b13975572848d416a4495db2bdc93dd926a3/src/box_picking_skills/aruco/aruco_pose_estimation.py|''pose_estimation_aruco'']]
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/camera_calibration/OHEC.py|''copy_tf'']]
	* ''wait'' (Already exists in system)
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/motion/random_point_on_sphere.py|''random_point_on_sphere_move'']]
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/motion/wait_for_velocity.py|''wait_for_velocity'']]
	* ''switch_controller'' (Already exists in system)
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/camera_calibration/ee_camera_pose_publisher.py|''ee_camera_pose_publisher'']]
	* [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/camera_calibration/hand_eye_calculation_opencv.py|''hand_eye_calculation_opencv'']]

The complete skill can be found [[https://git.cs.lth.se/robotlab/box_picking_skills/-/blob/rework_hec/src/box_picking_skills/camera_calibration/OHEC.py|here]].

===== TODO =====
	* Merge all relevant code to master branches, including **heron_launch**, and move relevant skills to correct packages as in //vision_skills// or //skills_sandbox//.
	* Implement so that fixed camera can be solved.