Inverse perspective mapping em python usando warpPerspective

Este artigo descreve as duas experiências no âmbito da visão artificial efectuadas numa simulação no gazebo com recurso ao método warpPerspective em python

Em python o método warpPerspective permite calcular como uma imagem seria vista de outra perspectiva.

dst = cv2.warpPerspective(src, transform_mat, dsize[, dst[, flags[, borderMode[, borderValue]]]] )

src: imagem original
transform_mat: matriz da transformação
dsize: tamanho da imagem gerada (w,h)
flags: (opcional) métodos de interpolação

A matriz da transformação é o ponto essencial sendo composta pelos elementos descritos na seguinte imagem.

O principal problema na aplicação deste método é a obtenção da matriz da transformação.

Para isso existem dois métodos no python, em ambos os casos são necessárias as coordenadas de 4 pontos, na imagem de origem e na imagem de destino.

  • transform_mat , status = cv2.findHomography(pts_src, pts_dst)
  • transform_mat = cv2.getPerspectiveTransform(pts_src, pts_dst)

# pts_src: coordenadas de 4 pontos na imagem original
# pts_dst: coordenadas desses 4 pontos na imagem transformada

No caso desta abordagem  para obter a vista superior (bird eye view) pelo método IPM, usaremos apenas o método getPerspectiveTransform.

Para descobrir as novas coordenadas dos quatro pontos foram colocadas em prática duas experiências no Gazebo.

Nestas experiências foi usado um programa de manipulação de perspectiva de modo a facilitar o calculo dos pontos. No entanto, o processo pode ser efectuado sem a ele se recorrer.

Experiência sem manter proporções

Foi montando o seguinte ambiente no gazebo. Um objecto rectangular e plano, de espessura desprezível, foi colocado à frente de uma viatura equipada com uma camera, de modo a ocupar todo o campo de imagem longitudinal.


imagem 1

Note-se que a quadricula tem 25 cm de lado, e que o centro da viatura e o centro da zona cinzenta estão sobre o mesmo eixo. A zona cinzenta tem 50 cm de largura. A imagem da camera existente da viatura, visualizada com o image view, é a seguinte.


imagem 2

Visualização com o programa de transformação, onde se constata que para os parâmetros exibidos a imagem é igual.


imagem 3

Visualização com o mesmo programa alterando apenas o parâmetro correspondente ao pitch


imagem 4

Com base na imagem 2 foram seleccionados os pontos dos 4 vertices do trapézio cinzento. Que têm as seguintes coordenadas, no sentido dos ponteiros do relógio, iniciando no topo superior esquerdo:

[200,0], [478,0], [200,479], [478,479]

Com base na imagem 4 foram seleccionados, os 4 pontos de correspondentes na imagem transformada.

[288, 0], [391, 0], [113, 479], [563, 479]

Estes pontos foram inseridos no seguinte programa (programa1), que quando executado, e conforme esperado, produziu uma imagem semelhante à da imagem 4.

import cv2
import numpy as np

img = cv2.imread('image2.jpg')


# define points coordinates
dst_pts = np.array([[200,0], [478,0], [200,479], [478,479]], dtype=np.float32)
src_pts = np.array([[288, 0], [391, 0], [113, 479], [563, 479]], dtype=np.float32)

# compute IPM matrix and apply it
transform_matrix = cv2.getPerspectiveTransform(src_pts, dst_pts)
ipm = cv2.warpPerspective(img, transform_matrix, img.shape[:2][::-1], flags=cv2.INTER_CUBIC)

# display image
cv2.imshow('ipm', ipm)
cv2.waitKey()

De seguida, aplicamos exactamente a mesma transformação à seguinte imagem.


image 5

O resultado da transformação obtida é o seguinte:


imagem 6

Nesta  imagem (6) verifica-se a compressão da distância que acontece na altura da imagem. Isto acontece porque, para o mesmo campo angular de visão, o deslocamento ao longo dos 45º de arco de circunferência, de A1 para A2, provoca a aproximação do ponto a a b, até serem coincidentes.

Esta situação está ilustrada na imagem seguinte.


imagem 7

Experiência mantendo as proporções

Foi montando o seguinte ambiente no gazebo. Um objecto quadrado e plano, com 1 m de aresta e de espessura desprezível, foi colocado à frente de uma viatura equipada com uma camera.


imagem 8

A aresta mais próxima do quadrado, dista 1 metro da projecção vertical da camera sobre a estrada.

A quadricula tem 25 cm de lado, e que o centro da viatura e o centro da zona cinzenta estão sobre o mesmo eixo.

Neste caso a imagem da camera existente da viatura, visualizada com o image view, é a seguinte.


imagem 9

Desta vez omitimos a exibição da imagem no programa de transformação, já que se sabe ser igual a imagem 9.

Quando se aplica um pitch de 37º (à pouco foi usado 36) o aspecto da imagem transformada fica o seguinte.


imagem 10

Neste programa de transformação não acontece o efeito de compressão, o efeito da compressão resultou da escolha, na experiência anterior de transformar os pontos apenas quanto à componente transversal da coordenadas, mantendo as coordenadas longitudinais iguais.

No entanto, o quadrado não aparece todo visível, pelo que se torna necessário ajustar a distancia do quadrado ao carro de modo a que se consiga ver todo.


image 11


image 12


image 13

Após procurar ajustar a distância do quadrado à viatura, ficando conforme visível nas imagens 11, 12 e 13, conclui que sem mudar outros parâmetros era impossível visualizar o quadrado de 1 metro na sua totalidade, pelo que reduzi o tamanho para 50 cm.


image 14


image 15

image 16

As imagens 14, 15 e 16, exibem, respectivamente, o ambiente no gazebo, a perspectiva da camera, e a perspectiva transformada.

Neste caso o quadrado é integralmente visível, no entanto, quando medido ao pormenor ele continua a estar comprimido na altura, já que o seu tamanho em pixels é de 279 x 259.

Aplicando ao programa1 os seguintes pontos:

source: [218, 191], [462, 191], [530, 374], [151, 374]
destination: [202, 145], [480, 145], [480, 404], [202, 404]

verificou-se que para a mesma imagem (imagem 15) correspondia uma imagem semelhante à da imagem 16.

Como a discrepância na dimensão dos pixels do quadrado é de 20 pixels, foi efectuada uma alteração na coordenada y dos dois primeiros pontos de destino. Globalmente ficou assim.

destination: [202, 125], [480, 125], [480, 404], [202, 404]

O resultado melhorou bastante, e o quadrado ficou com 279 x 279 px, pelo que também se pode  concluir que nesta vista existem 558 pixels por metro.

Aplicando esta nova transformação a imagem 5, obteve-se então a seguinte imagem que está menos comprimida.

Conclusão, para obter empiricamente as coordenadas dos pontos dos quatro pontos na imagem de origem e respectivas as coordenadas dos pontos na imagem de destino, deve-se usar um quadrado de dimensões conhecidas que esteja inteiramente visível.

Referencias

https://theailearner.com/tag/cv2-warpperspective/

Geometry of Image Formation

https://www.edmundoptics.com/knowledge-center/application-notes/imaging/understanding-focal-length-and-field-of-view/

 

Using the calibration file

Using the calibration file

On the ROS calibration procedure, when we click the commit button, the node write a file to the ~/.ros/camera_info, with the name of the camera.

As far as i know today we may control the name of the camera adding the following code (need to check)

<param name="camera_name" value="vx800_camera" />

Depois de o ficheiro estar gravado, podemos fazer uma cópia do ficheiro para um pacote do ROS, de modo a que o possamos usar mais tarde, e ao mesmo tempo não correr o risco de perder o ficheiro por sobreposição de um ficheiro de calibração de outra camera com o mesmo nome.

Para isso deveremos acrescentar as seguintes linhas ao ficheiro launch que usamos para lançar o node com o driver da usb_cam.

<param name="camera_info_url" value="file:///$(find sibot_video)/camera/ms_vx800_camera.yaml" />
<param name="camera_name" value="vx800_camera" />

Globalmente o ficheiro deverá ficar parecido com este, substituindo os nomes e topicos pelos que forem apropriados.

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video2" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
    <param name="camera_info_url" value="file:///$(find sibot_video)/camera/ms_vx800_camera.yaml" />
    <param name="camera_name" value="vx800_camera" />

  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

Quando o node é lançado com estas informações estas são usadas para preencher o tópico: camera/camera_info

 

ROS_NAMESPACE=usb_cam rosrun image_proc image_proc

On a launch file we may use the following code to launch the image_proc node.

<node name="image_proc" pkg="image_proc" type="image_proc" ns="usb_cam"/>

 

http://wiki.ros.org/image_proc

 

microsoft lifecam vx-800 specs and calibration file

Microsoft lifecam vx-800 specs

Imaging Features: Fixed focus Automatic image adjustment with manual override

Sensor: CMOS VGA sensor technology

Res. Motion Video: 0.31 megapixel (640 x 480 pixels)*
Res. Still Image: 0.31 megapixel (640 x 480 pixels) without interpolation*

Field of View: 59° diagonal field of view

Specifications PDF: TDS_LifeCamVX-800

https://en.wikipedia.org/wiki/LifeCam

ROS Calibration

Command to launch the calibration procedure

python3 /opt/ros/noetic/lib/camera_calibration/cameracalibrator.py –size 8×6 –square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

Console output for the calibration procedure

mono pinhole calibration...
D = [-0.024968869261289526, 0.07930822220599683, -0.003910806144872592, -0.004521345730832268, 0.0]
K = [682.7339388231446, 0.0, 350.24230621129846, 0.0, 685.0091845549854, 234.69855008454573, 0.0, 0.0, 1.0]
R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P = [684.7103271484375, 0.0, 347.0226591393148, 0.0, 0.0, 687.8898315429688, 232.61935265769353, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters

[image]

width
640

height
480

[narrow_stereo]

camera matrix
682.733939 0.000000 350.242306
0.000000 685.009185 234.698550
0.000000 0.000000 1.000000

distortion
-0.024969 0.079308 -0.003911 -0.004521 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
684.710327 0.000000 347.022659 0.000000
0.000000 687.889832 232.619353 0.000000
0.000000 0.000000 1.000000 0.000000

ROS Calibration File

ms_vx800_camera

 

Camera calibration gazebo

Camera calibration gazebo

Ultimamente tenho usado mais o gazebo para simular os robots e os diversos ambientes em que eles podem actuar.

A utilização que tenho feito do gazebo tem sido essencialmente orientada para o desenvolvimento de soluções para a viatura robótica do automec que será usada para competir nas provas de condução autónoma no Festival Nacional de Robótica.

Ao captarem a realidade as cameras introduzem distorção na representação que é feita. Essa distorção torna-se bastante visível quando se usa quadros de xadrez nas tomadas de imagem.

Apesar de ter lido que no gazebo a camera está pré calibrada e que não necessita de ser calibrada[1]  pelo processo usado em cameras reais, uma investigação breve sobre IPM (Inverse Perspective Mapping), deixou claro que a utilização do quadro de xadrez é um elemento essencial para encontrar os parâmetros da transformação, e que todos os processos descritos incluíam a fase de calibração da camera, pela que a reprodução deste processo no gazebo torna-se importante.

Além disso também podemos definir os parâmetros intrínsecos no xml que define a camera [2].

Após uma pesquisa na web encontrei o pacote  calibration_gazebo que após experimentar verifiquei que funcionava bem num dos exemplos que tinha integrado, mas que quando aplicado ao ambiente simulado usado no projecto do automec não funcionava.

Como alternativa usei apenas ficheiro sdf que define o quadro de xadrez, integrando-no pacote que define o ambiente de simulação.

Desta forma o quadro de xadrez fica disponível para inserção no ambiente simulado recorrendo a facilidade de drag and drop do gazebo.

Para o efeito cria-se uma pasta chamada chessboard, dentro da pasta models do pacote que define o ambiente simulado, no caso do automec (simulation_environment)

De seguida, copia-se os seguintes ficheiros para dentro da pasta criada. O ficheiro xacro serve apenas para gerar o ficheiro sdf, caso desejemos que o xadrez tenha outras características, como por exemplo quadrados de tamanho diferente.

  • https://github.com/inaciose/calibration_gazebo/blob/master/sdf/landmark.sdf
  • https://github.com/inaciose/calibration_gazebo/blob/master/sdf/landmark.sdf.xacro

Muda-se o nome nos ficheiros para chessboard, assim como todas as ocorrencias de landmark no seu conteúdo.

Nessa mesma pasta cria-se o ficheiro model.config com o seguinte conteúdo.

 
<model> 
  <name>chessboard</name> 
  <version>1.0</version> 
  <sdf version="1.4">
  chessboard.sdf
  </sdf>
  <description>
    Chessboard.
  </description>
</model>

No ficheiro package.xml do pacote, caso não esteja já definido deveremos acrescentar as seguintes linhas.

  <export>
    <gazebo_ros gazebo_model_path="${prefix}/models"/>
  </export>

As características do xadrez pré definido são:

  • quadrados (vertices internos): 8×6
  • tamanho: 0.03

Para colocar o quadro de xadrez no ambiente simulado basta arrastar a sua designação para a posição adequada (em frente da camera). Roda-lo de forma aos quadrados ficarem voltados para a camera. Durante o procedimento de calibragem, devemos desloca-lo e roda-lo até ficar disponível o botão Calibrate .

O seguinte comando dá inicio ao processo de calibragem. Nesse comando há que substituir o nome do tópico e da camera pelos correctos

rosrun camera_calibration cameracalibrator.py –size 8×6 –square 0.03 image:=/image_topic camera:=/camera

No caso do ROS noetic há que lançar o node da seguinte forma:

python3 /opt/ros/noetic/lib/camera_calibration/cameracalibrator.py –size 8×6 –square 0.03 image:=/image_topic camera:=/camera

Quando este comando é executado aparece uma janela onde fica visível a imagem captada pela camera, e quando é caso disso a indicação dos vertices identificados pelo programa.

Alem de 3 botões desligados  (calibrate, save e commit) que se activam quando o programa capturar uma serie de imagem do quadro de xadrez com os vertices identificados.

Vai ser necessário identificar esses vertices algumas vezes, de vários angulos e distancias, sendo que é exibida na consola uma linha com os dados adquiridos em cada posição e orientação considerada.

Quando estiver activo, deve-se clicar no botão Calibrate, surgindo na consola os parâmetros de calibragem calculados recolhidos.

Posteriormente clicar no botão save para guardar um ficheiro de arquivo (/tmp/calibrationdata.tar.gz) com as várias imagens consideradas no processo de calibragem, e dois ficheiros com os parâmetros para calibrar a camera (ost.txt e ost.yaml)

Quando se clica no botão commit, é criado um ficheiro camera.yaml na seguinte pasta: ~/.ros/camera_info

O conteúdo do ficheiro criado nesta pasta é igual ao do ficheiro ost.yaml, e contêm os parâmetros de calibração da camera.

[1]
https://answers.ros.org/question/28957/calibrate-gazebo-camera/

[2]
https://answers.ros.org/question/11965/using-camera-sensor-in-gazebo/

ackerbot early building

Video da base para carro robot com direção ackermann

Numa primeira fase imaginei um robot com direção ackermann com um diferencial traseiro e um motor dc disposto de forma longitudinal relativamente ao chassis.

Para isso comprei no aliexpress um conjunto de peças que implementam uma direção ackermann, e um diferencial traseiro.

Numa impressora 3D fiz uns adaptadores (com rolamento) para as rodas da frente, e uns suportes de eixo (com rolamento) a aplicar com os eixos traseiros.

Pormenor das ligações do diferencial traseiro ao motor e ao eixo das rodas

 

dav
dav
dav
dav
dav

ackerbot hw v1.1 rms 1st tests

As experiencias de treino e condução da nova versão do hardware do ackerbot foram efectuadas em duas partes.

Na primeira parte, foi usada a camera USB já existente.  na segunda parte foi usada a nova camera csi de 160º de FOV

 

ackerbot hw v1.1 description and first tests

Yesterday, 27/08, i tried the upgraded ackerbot.

The new hardware version have the ESP8266 replaced by an ESP32, and new modules. An IMX219 camera, an IMU MPU6050, a smal OLED screen, connection available to a GPS (with basic software already tested on a breadboard), and an 3.3V power supply by an LM317T.

 

After some successfully preliminar tests, i try to free drive it to my living room, but i lost the robot control because the bad WiFi coverage.

After that event all my rosserial connections ended on error. See the terminal output bellow.

[INFO] [1630082983.539943]: Setup subscriber on pub_vel [std_msgs/Int16]
[WARN] [1630082997.385001]: Last read step: data checksum
[WARN] [1630082997.389949]: Run loop error: [Errno 104] Connection reset by peer
[INFO] [1630082997.395216]: Removing subscriber: pub_dir
[INFO] [1630082997.405242]: Removing subscriber: pub_vel
[INFO] [1630082997.413336]: Removing subscriber: cmd_vel
[INFO] [1630082997.421426]: Shutting down
[INFO] [1630082997.425133]: All done
Traceback (most recent call last):
File "/opt/ros/melodic/lib/rosserial_python/serial_node.py", line 73, in <module>
server.listen()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rosserial_python/SerialClient.py", line 253, in listen
self.startSerialClient()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rosserial_python/SerialClient.py", line 259, in startSerialClient
client.run()
File "/opt/ros/melodic/lib/python2.7/dist-packages/rosserial_python/SerialClient.py", line 556, in run
self.port.flushOutput()
AttributeError: RosSerialServer instance has no attribute 'flushOutput'

Today, the first connection, is successfully and stable. I believe that the yesterday issues are due to the wifi connections.

My home have a WiFi extender. The extender have the _EXT suffix on the ssid. If the main ssid is myssid, the extender have myssid_EXT.

 

 

ROS noetic Kinect Xbox 360

$ lsb_release -a
No LSB modules are available.
Distributor ID: Ubuntu
Description: Ubuntu 20.04.3 LTS
Release: 20.04
Codename: focal

The software required to explore the kinect xbox 360 in ROS must be compiled from sources.

First we need to build the libfreenect (no ros package) from sources, and only after that can build the freenect_stack (ros package) also from the sources.

We also need to install the ros package rgbd-launch with the following command

sudo apt-get install ros-noetic-rgbd-launch

 

build libfreenect

sudo apt-get update
sudo apt-get upgrade
sudo apt-get install git-core cmake freeglut3-dev pkg-config build-essential libxmu-dev libxi-dev libusb-1.0-0-dev

cd ~/src
git clone git://github.com/OpenKinect/libfreenect.git
cd libfreenect
mkdir build
cd build
cmake -L ..
make
sudo make install
sudo ldconfig /usr/local/lib64/

build freenect_stack

cd ~/catkin_ws/src
git clone https://github.com/ros-drivers/freenect_stack.git
cd ..
catkin_make

run freenect node

roslaunch freenect_launch freenect.launch depth_registration:=true

Explore kinect published topics

rviz

The kinect xbox 360 output topics can be visualized on rviz  with the following displays:

  • Image
  • Camera
  • Pointcloud2
  • DeepCloud

In the ‘Global Options’ set the ‘Fixed Frame’ to ‘camera_link’.

The last two displays have configurations to give colors to points, superimposition of outputs, etc.

Not all topics produce visible output.

Error building freenect_stack

-- Checking for module 'libfreenect'
-- No package 'libfreenect' found
CMake Error at /usr/share/cmake-3.16/Modules/FindPkgConfig.cmake:463 (message):
A required package was not found

Solution: build libfreenect before

Error building freenect_stack

Resource not found: rgbd_launch
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/inaciose/catkin_ws/src
ROS path [2]=/opt/ros/noetic/share
The traceback for the exception was written to the log file

solution: sudo apt-get install ros-noetic-rgbd-launch

Reference

Give your next Robot 3D vision: Kinect V1 with ROS Noetic

ROS GPS with tinygps and rosserial

Exploração do sistema GPS no ROS.

Esta página reúne alguma informação recolhida na minha pesquisa e experiências com os dispositivo de localização GPS uBlox neo 6M e neo M8N no ROS.

uBlox neo-6M chip in one of the used modules

As experiências foram efectuadas num ESP32 de 38 pinos, com a ligação serial no RX2 e TX2, e alimentação de 3.3V.

ESP32 – 38 pins pinout

A exploração das informações recolhidas do GPS foi efectuada com a biblioteca tinygpsplus.

http://arduiniana.org/libraries/tinygpsplus/

A programação necessária foi efectuada no vscode com o platformio.

NMEA sentences

As informações go GPS são obtidas com base em fluxos de texto cujas linhas começam com as seguintes sequências de caracteres e cada uma delas agrupa informações especificas.

$GPRMC
$GPVTG
$GPGGA
$GPGSA
$GPGSV
$GPGLL

No caso do neo6M verifiquei que existem 3 linhas seguidas da $GPGSV

https://web.fe.up.pt/~ee95080/NMEA%20data.pdf

https://www.trimble.com/oem_receiverhelp/v4.44/en/NMEA-0183messages_MessageOverview.html

 

ESP32 tinygps rosserial

Este repositório contem o software que publica mensagens GPS num tópico do ROS.

https://github.com/inaciose/esp32_ard_rosserial_gps

Neste momento o tipo de mensagem publicada é a GPSFix, por ser a mais completa.

Assim que possível serão feitas as alterações necessitarias para poder escolher entre mensagens GPSFix e NavSatFix.

ROS messages

Existem dois tipos de mensagens usados na publicação das informações do GPS:

  • NavSatFix
  • GPSFix

O pacote gps_common contem um script para publicar conversões entre estes dois tipos de mensagens.

https://github.com/swri-robotics/gps_umd/blob/master/gps_common/nodes/fix_translator

Mensagens NavSatFix

Estas mensagens fazem parte do grupo das sensor_msgs e incluem, para além da longitude, latitude e altitude, informações do tipo NavSatStatus (que contẽm informaçoes sobre os satélites)

Este tipo de mensagens contêm menos informações que as do tipo GPSFix.

  • https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatFix.html
  • https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatStatus.html

Mensagens GPSFix

Estas mensagens fazem parte do grupo das gps_common, definidas no pacote com o mesmo nome e incluem mais informações que as mensagens do tipo NavSatFix, também incluem informações do tipo GPSStatus (que contẽm informaçoes sobre os satélites)

  • http://docs.ros.org/en/api/gps_common/html/msg/GPSFix.html
  • http://docs.ros.org/en/api/gps_common/html/msg/GPSStatus.html

Algumas particularidades na publicação da mensagem GPSFix

Devido ao limite do  buffer do rosserial, o numero de satélites visíveis sobre os quais é publicada informação está limitado a 8 . Neste momento não estão a ser seleccionados todos os satélites em uso, o limite é por ordem no array.

A mensagem de erro na consola do ROS é a seguinte:

[ERROR] [1631463315.015253]: Message from device dropped: message larger than buffer.

Pelo que percebi, os satélites são aqueles cujo SNR é zero.

Outra particularidade é como preencher campos de mensagens ros que são arrays variáveis.

Segundo as informações no ros wiki (http://wiki.ros.org/msg) os arrais de inteiros são preenchidos com variáveis do tipo std::vector<T>, mas no rosserial este tipo não está disponível.

Para contornar este problema devemos, fazer com que o campo aponte para um array pré declarado, e posteriormente  atribuir o numero de itens no array a um campo não documentado (msg.campo_lenght) na definição do tipo da msg do ros.

int array[MAX_ITENS];
(put values in the array)
gps_status_msg.satellite_visible_prn = array;
gps_status_msg.satellite_visible_prn_length = array_item_count;

GPSFix – Informações não preenchidas ou por confirmar

# Direction (degrees from north)
float64 track – gps.course.deg() ??? is from north?

# Vertical speed (meters/second)
float64 climb

# Device orientation (units in degrees)
float64 pitch
float64 roll
float64 dip

# Total (positional-temporal) dilution of precision
float64 gdop

# Temporal dilution of precision
float64 tdop

# Uncertainty of measurement, 95% confidence
all err_ and covariance information

GPSStatus – Informações não preenchidas

# Satellites used in solution
int32[] satellite_used_prn # PRN identifiers

uint16 motion_source # Source for speed, climb and track
uint16 orientation_source # Source for device orientation
uint16 position_source # Source for position

 

ROS packages

gps_umd

https://wiki.ros.org/gps_umd (meta package)

https://github.com/swri-robotics/gps_umd

  • gps_common
  • gpsd_client

gpsd_viewer

http://wiki.ros.org/gpsd_viewer

nmea_navsat_driver

http://wiki.ros.org/nmea_navsat_driver

https://github.com/ros-drivers/nmea_navsat_driver

nmea_msgs

http://wiki.ros.org/nmea_msgs

https://github.com/ros-drivers/nmea_msgs

 

Links

https://answers.ros.org/question/327193/how-have-a-static-map-to-understand-its-gps-coordinates/

https://answers.ros.org/question/327193/how-have-a-static-map-to-understand-its-gps-coordinates/

https://answers.ros.org/question/220368/using-mapviz-gpsfix-vs-navsatfix/

https://medium.com/@hitlx916/visualize-the-gnss-messages-in-mapviz-ros-4ae7eec19936

http://wiki.ros.org/mapviz

https://github.com/nobleo/rviz_satellite

 

Jetson nano csi camera on ROS

There are some ROS packages for using CSI cameras on ROS

https://github.com/sfalexrog/jetson_camera

https://github.com/peter-moran/jetson_csi_cam

I try both but only get good results with the first one. So I forked it on the following addres:

https://github.com/inaciose/jetson_camera

 

jetson_camera

ROS package for csi camera video publisher

roslaunch jetson_camera jetson_camera.launch

There also a simple node to capture and publish the CSI camera image.

https://github.com/inaciose/ackerbot/blob/8d1851bf0a04c5f8a2c333b18d5b47b5c3486391/src/csicam_publisher.cpp

 

Camera calibration

Doing camera calibration with jetson_camera package publishing video from a IMX219 camera.

A simples utilização do ficheiro de calibração no driver não rectifica a imagem. Para efectuar a retificação da imagem usar o pacote image_proc.

Investigar melhor no futuro a optimização da calibração de cameras de olho de peixe.

http://wiki.ros.org/camera_calibration

http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration

Commands

(terminal 1)
roslaunch jetson_camera jetson_camera.launch

(terminal 2)
rosrun camera_calibration cameracalibrator.py –size 9×6 –square 0.0265 image:=/main_camera/image_raw camera:=/main_camera

Problems running cameracalibrator.py

ImportError: No module named cv2

Python version error

The solution is changing the shebang to python3

alternative is direct runing

python3 /opt/ros/noetic/lib/camera_calibration/cameracalibrator.py –size 8×6 –square 0.108 image:=/camera/image_raw camera:=/camera

Service set_camera_info error

Waiting for service /main_camera/set_camera_info …
Service not found
QMutex: destroying locked mutex
QMutex: destroying locked mutex
Segmentation fault (core dumped)

O comando rosservice list não exibe nenhum serviço set_camera_info.

Execução com novo parametro

Globalmente, o processo de calibração seguido está descrito na wiki ros MonocularCalibration, no caso da camera csi com o jetson_camera é necessário incluir um parâmetro na chamada ao node cameracalibrator.py.

Conforme vi referido numa página web (noservice) , o node permite usar o parâmetro –no-service-check, que inibe a verificação dos serviços.

rosrun camera_calibration cameracalibrator.py –size 9×6 –square 0.0265 –no-service-check image:=/main_camera/image_raw camera:=/main_camera

Com este comando exibiu a seguinte janela.

Na janela é possível configurar o tipo de camera e a escala.

Como a camera tem um angulo de 160 graus penso que será uma camera olho de peixe.

Entretanto verifiquei no wiki ros camera calibration que a partir do melodic existem uma serie de parâmetros destinados a cameras olho de peixe.

–fisheye-recompute-extrinsicsts
–fisheye-fix-skew for fisheye
–fisheye-fix-principal-point
–fisheye-k-coefficients=NUM_COEFFS
–fisheye-check-conditions

De qualquer modo avancei com as seguintes configurações:

Pinhole = 1/1

Scale = 000/100

O interface tem 3 botões, Calibrate, Save e Commit.

Pelo que percebi o botão Commit só funciona bem se o serviço set_camera_info estiver disponível.

O botão Calibrate fica disponível depois de passarmos a imagem com os quadrados brancos e pretos, e os seus vertices em contacto serem detectados várias vezes.

Quando disponível cliquei no botão Calibrate e um conjunto de dados foram exibidos no terminal. Ver output de calibração.

De seguida cliquei em Save, e o seguinte ficheiro de arquivo ficou disponível

/tmp/calibrationdata.tar.gz

Deste arquivo destaco os seguintes ficheiros

ost.txt
ost.yaml

Estes ficheiros contêm os dados constantes no output de calibração. Em particular o ost.txt, é a segunda parte desse output.

O ficheiro ost.yaml contem os mesmos dados de calibração já no formato yaml que é usado no argumento camera_info_url nos drivers de camera para o ros.

Interrompi o funcionamento do node de captura de imagem e o node de calibração da camera.

Copiei o ficheiro para uma pasta, (~/.ros/camera_calibration)

Alterei o  parametro camera_info_url no ficheiro jetson_camera.launch para usar o esse ficheiro de calibração.

Quando lancei o jetson_camera.launch apareceu o seguinte warning.

[ WARN] [1630073534.594131929]: [camera] does not match name narrow_stereo in file /home/inaciose/catkin_ws/src/jetson_camera/camera_info/csi_cam_160_cal.yaml

A solução é trocar no ficheiro ost.yaml (antes de o copiar) o nome da camera para o nome certo. No caso:

# de
camera_name: narrow_stereo
# para
camera_name: camera

 

Still working on it

Usefull commands

Comandos uteis na calibração de uma camera IMX219 com o pacote jetson_camera.

roslaunch jetson_camera jetson_camera.launch

rosrun camera_calibration cameracalibrator.py –size 9×6 –square 0.0265 image:=/main_camera/image_raw camera:=/main_camera

rosrun camera_calibration cameracalibrator.py –size 9×6 –square 0.0265 –no-service-check image:=/main_camera/image_raw camera:=/main_camera

rosservice list

rosnode list

rostopic list

 

Extra urls

http://ros.org/wiki/camera_info_manager

http://ros.org/wiki/camera_calibration_parsers

http://wiki.ros.org/image_proc

Calibrar cameras olho de peixe
https://sites.google.com/site/scarabotix/ocamcalib-toolbox

Image pipeline alternativo
https://github.com/DavidTorresOcana/image_pipeline

Exemplos de mensagens nos topicos

rostopic echo /main_camera/camera_info

header: 
  seq: 5781
  stamp: 
    secs: 1630064629
    nsecs: 882128788
  frame_id: "main_camera_optical"
height: 480
width: 640
distortion_model: ''
D: []
K: [nan, 0.0, nan, 0.0, nan, nan, 0.0, 0.0, 0.0]
R: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
P: [nan, 0.0, nan, 0.0, 0.0, nan, nan, 0.0, 0.0, 0.0, 0.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

Output de calibração

**** Calibrating ****
mono fisheye calibration...
D = [-0.16411803209719655, 0.5323506420170566, -0.667575196157337, 0.3072524695696274]
K = [217.77569928178522, -3.0709848224418317, 315.6222686786177, 0.0, 285.57107620446374, 240.32213346777095, 0.0, 0.0, 1.0]
R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P = [217.77569928178522, -3.0709848224418317, 315.6222686786177, 0.0, 0.0, 285.57107620446374, 240.32213346777095, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
217.775699 -3.070985 315.622269
0.000000 285.571076 240.322133
0.000000 0.000000 1.000000

distortion
-0.164118 0.532351 -0.667575 0.307252

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
217.775699 -3.070985 315.622269 0.000000
0.000000 285.571076 240.322133 0.000000
0.000000 0.000000 1.000000 0.000000

('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')

Sources

noservices
https://answers.ros.org/question/62778/ros_camera_info-service-not-found-for-usb_cam/

 

No caso de uma camera usb (usb_cam), quando se clica no botão Commit, sai-se do programa e o node de captura da camera usb exibe a seguinte linha.

 

[ INFO] [1630076380.945883171]: writing calibration data to /home/inaciose/.ros/camera_info/head_camera.yaml