Integrate frame_alignment calibration

[miguelriemoliveira]

This is the kickoff email from @manuelgitgomes ,

Envio em anexo um dataset.json da minha calibração de contacto.

Passo agora a explicar os pontos principais.
A definição de sensores é feita no "calibration_config" na secção
"contact_sensors":

 "contact_sensors": {
   "left_hand_sensor": {
     "contact_topic": "/atom/left_contact",
     "touched_link": "closest",
     "touching_link": "touch_probe_cylinder_top_frame",
     "touching_visual_link": null
   },
   "right_hand_sensor": {
     "contact_topic": "/atom/right_contact",
     "touched_link": "closest",
     "touching_link": "rh_ff_biotac_link",
     "touching_visual_link": "rh_ffdistal"
   }
 },

Aonde "contact_topic" é o tópico que envia uma mensagem de contacto.
Cada mensagem de contacto tinha apenas um header (frame_id do sensor e
stamp) e a posição do contacto, que podia ser detetada pelos sensors
biotac, ou era mantida a zero no caso do outro sensor.

Depois tinha o "touched_link", em que se podia definir qual o link que o
sensor estava a tocar. No caso de closest, ele iria procurar o mais
próximo a cada coleção e variava assim de coleção para coleção.
"touching_link" refere-se ao link do sensor, tendo de ser o mesmo que o
frame_id da mensagem de contacto.

O "touching_visual_link" era porque era preciso saber o link do sensor
que tinha a sua visualização.
Isto era para o poder remover de possíveis links no "closest touched_link".

Uma coleção com informação de contacto seguia o seguinte formato:

   "contact": {
     "contact_pose": {
       "orientation": [
         0.0,
         0.0,
         0.0,
         0.0
       ],
       "position": [
         0.0,
         0.0,
         0.0
       ]
     },
     "contact_sensor": "left_hand_sensor",
     "frame_id": "touch_probe_cylinder_top_frame",
     "touched_link": "rh_forearm"
   },

Ao invés de ter um "labels", tem um "contact" (a parte que falámos que
poderia ser unificada).
Este campo apenas tem o nome do sensor ("contact_sensor"), informações
que vinham na mensagem de contacto ("frame_id" e "contact_pose").
O "touched_link" é o mesmo da configuração, mas desta vez resolvido,
i.e., se na configuração foi escolhido "closest", aqui já aparece o link
mais próximo.

Envio também aqui o link para o meu fork do atom:

https://github.com/manuelgitgomes/tactile-calibration

Destaco o código de calibração:

if dataset['calibration_config']['contact_sensors']\

[collection['contact']['contact_sensor']]['touched_link'] == 'closest':
distance, touched_contact_point =
getClosestDistanceFromLinkToMesh(collection, meshes,
collection['contact']['frame_id'],
contact_position_array,
collection['contact']['touched_link'],
dataset['calibration_config']['world_link'])
r[rname] = distance / normalizer['contact']

     [...]
     else:
         contact_sensor_to_touched_link = getTransform(
                     collection['contact']['frame_id'],
                     collection['contact']['touched_link'],
                     collection['transforms'])

         contact_pose_in_touched_link =

np.dot(contact_sensor_to_touched_link, contact_position_array)
# print(contact_pose_in_touched_link)
r[rname] =
np.linalg.norm(contact_pose_in_touched_link[0:3]) / normalizer['contact']

[miguelriemoliveira]

So I think that the first point is that we should think of two types of calibration. @manuelgitgomes had them together but I think its preferable to separate them.

• Contact calibration, i.e., when a sensor touches a surface
• Manual alignment of two reference frames

My proposal is to address the manual alignment only for now.

For this purpose I will add to the frame_alignment modality to the sensor data in the calibration.yaml.

[miguelriemoliveira]

Now that I think better about it I do not think that the data can be just another modality of a sensor. In the objective function we iterate sensors together with patterns and this has no calibration pattern associated ...

So the best is I think to create a new field contacts at the same level of sensors.