Blog

Assume that you happened to know the coordinates of your sensor in some Cartesian coordinate system, and you want to derive the coordinates of your robot centre. For the robot configuration you know the offset of the sensor from the robot centre, but not the other way around. The solution:

get inverse offset
# assume this is the offset of the sensor from the robot centre
offset="1.3,-0.6,0.3,-0.5,0.6,-0.7"

inversed_offset=$( echo "0,0,0,0,0,0" | points-frame --to="$offset" --fields="x,y,z,roll,pitch,yaw" --precision=16 )

Now inversed_offset is the position (and pose) of the robot centre in the coordinate system associated with the sensor.

Step by step demo:

  • Start with some coordinates (navigation data in the world frame; the specific coordinate system does not matter). A sample data file is attached to this page:

    get nav
    cat nav.bin | csv-from-bin t,6d | head -n 2
    20101209T032242.279632,6248546.875440197,332966.0202201727,-37.76910082437098,-0.02045280858874321,-0.01195328310132027,2.113722085952759
    20101209T032242.280425,6248546.875484069,332966.020129519,-37.76909669768065,-0.02046919427812099,-0.0119620431214571,2.113716840744019

    The example uses binary, but this is up to you. The nav.bin file contains the trajectory of the robot centre (GPS unit) in the world frame.

  • Get the coordinates of the sensor in the world frame:

    get sensor trajectory
    cat nav.bin | csv-paste "-;binary=t,6d" "value=$offset;binary=6d" \
        | points-frame --from --fields=",frame,x,y,z,roll,pitch,yaw" --binary="t,6d,6d" \
        | csv-shuffle --fields="t,,,,,,,,,,,,,x,y,z,roll,pitch,yaw" --binary="t,6d,6d,6d" --output-fields="t,x,y,z,roll,pitch,yaw" > sensor.bin

    Now sensor.bin is the trajectory of the sensor in the world frame. We want to get the trajectory of the robot centre from these data.

  • Just do it:

    get centre coordinates back
    cat sensor.bin | csv-paste "-;binary=t,6d" "value=$inversed_offset;binary=6d" \
        | points-frame --from --fields=",frame,x,y,z,roll,pitch,yaw" --binary="t,6d,6d" \
        | csv-shuffle --fields="t,,,,,,,,,,,,,x,y,z,roll,pitch,yaw" --binary="t,6d,6d,6d" --output-fields="t,x,y,z,roll,pitch,yaw" > restored.bin

    Note the use of inversed_offset.

  • Verify by comparing to the original nav.bin:

    verify
    cat nav.bin \
        | csv-paste "-;binary=t,6d" "restored.bin;binary=t,6d" \
        | csv-eval --full-xpath --binary="t,6d,t,6d" --fields="f/t,f/x,f/y,f/z,f/roll,f/pitch,f/yaw,s/t,s/x,s/y,s/z,s/roll,s/pitch,s/yaw" \
            "dx = abs(f_x - s_x); dy = abs(f_y - s_y); dz = abs(f_z - s_z); droll = abs(f_roll - s_roll); dpitch = abs(f_pitch - s_pitch); dyaw = abs(f_yaw - s_yaw);" \
        | csv-shuffle --fields=",,,,,,,,,,,,,,dx,dy,dz,droll,dpitch,dyaw" --binary="t,6d,t,6d,6d" --output-fields="dx,dy,dz,droll,dpitch,dyaw" \
        | csv-calc --fields="dx,dy,dz,droll,dpitch,dyaw" --binary="6d" mean \
        | csv-from-bin 6d

    The output is on the order of. The precision is defined by the accuracy of inversed_offset calculations above. If the --precision=16 option were not given, the comparison would be valid up toor so.

Rabbit MQ

Introduction

Rabbit MQ is an open source message queue service (https://www.rabbitmq.com/).

It implements AMQP 0-9-1 (https://www.rabbitmq.com/tutorials/amqp-concepts.html).

programming tutorials: https://www.rabbitmq.com/getstarted.html

Installation

For Ubuntu:

 

sudo apt-get install rabbitmq-server
# check service is installed and running
service rabbitmq-server status
# for python clients
sudo pip install pika

 

(for other platforms see installation from the website)

rabbit-cat

rabbit-cat is a light rabbit MQ client in python available in comma.

see rabbit-cat -h for examples

example 1

For receiver run:

 

rabbit-cat listen localhost --queue="queue1"

 

For sender in a separate terminal:

 

echo "hello world!" | rabbit-cat send localhost --queue="queue1" --routing-key="queue1"

 

 

When joining two point clouds, if you would like to output a few nearest points, now you can use points-join with --size option:

> # single nearest point (same as before):
> echo 0,0,0 | points-join <( echo 0,0,1; echo 0,0,2; echo 0,0,3 ) --radius 5
0,0,0,0,0,1
> # up to a given number of nearest points:
> echo 0,0,0 | points-join <( echo 0,0,1; echo 0,0,2; echo 0,0,3 ) --radius 5 --size 2
0,0,0,0,0,1
0,0,0,0,0,2

Suppose, the GPS unit on a vehicle is offset from the vehicle geometrical centre. Therefore, you most likely need to convert the GPS trajectory as 6DOF points (x,y,z,roll,pitch,yaw) to the trajectory of the vehicle centre.

The other (almost identical) use case: you have got a trajectory from Visual SLAM relative to a lidar and now want to convert it into the vehicle centre trajectory.

Now, it can be done as following:

> gps_unit_offset=1,2,3,0.1,0.2,0.3
> cat gps_unit_trajectory.csv | points-frame --position $gps_unit_offset --fields frame

In the past, in such a situation, one would need to jump through the hoops with points-frame as following:

> cat gps_unit_trajectory.csv | csv-paste - value=$gps_unit_offset | points-frame --fields frame,position | csv-shuffle --fields ,,,,,,,,,,,,x,y,z,roll,pitch,yaw --output-fields x,y,z,roll,pitch,yaw

Suppose, you have two point clouds cloud 1 and cloud 2. Suppose, for each point P from cloud 1 you would like to get all the points from cloud 2 that are not farther then a given radius from P

Then, you could use points-join --all

> cat cloud-1.csv | points-join cloud-2.csv --radius 1.5 --all

Now, you also can specify variable radius for points in cloud 1. (E.g. your radius may vary depending on your point cloud density or structure, as it happened in our use case.)

Then you could run:

> cat cloud-1.csv | points-join --fields x,y,z,radius cloud-2.csv --radius 1.5 --all

(Note that, as a limitation, the point-specific radius should not exceed --radius value.)

Sometimes, when you run some slow processing of a point cloud and output the result in a file, you may want to monitor the progress. Then, the following trick may help you:

> some_slow_processing_script > points.csv &
> ( i=0; while true; do cat points.csv | csv-paste value=$i -; (( ++i )); done; sleep 30 ) | view-points --fields block,,,,x,y,z

Suppose, you need to go through a dataset to pick images for your classification training data.

cv-cat view can be used for basic browsing, selecting images, and assigning a numeric label to them:

cat images.bin | cv-cat "view=0,,png"

The command above will show the image and wait a key press:

Press whitespace to save the file as <timestamp>.png, e.g. 20170101T123456.222222.png

Press numerical keys 0-9: save the file as <timestamp>.<num>.png, e.g. if you press 5: 20170101T123456.222222.5.png

Press <Esc> to exit.

Press any other key to show the next frame without saving.

 

view parameters have the following meaning:

The first parameter is the wait in milliseconds for key press, 0 to wait indefinitely.

The second parameter is the window title (irrelevant for labelling).

The third parameter is the image extension e.g. png, jpg ...; default ppm.

 

A few new features have been added to cv-cat accumulate filter.

Before, it accumulated images as sliding window of a given size. Now, you could also ask for fixed layout of the accumulated image. It sounds confusing, but try to run the following commands (press any key to move to the next image)

> # make sense of the input
> ( yes 255 | csv-to-bin ub ) | cv-cat --input 'no-header;rows=64;cols=64;type=ub' 'count;view=0;null'
> # accumulate as sliding window of size 4
> ( yes 255 | csv-to-bin ub ) | cv-cat --input 'no-header;rows=64;cols=64;type=ub' 'count;accumulate=4;view=0;null'
> # accumulate as sliding window of size 4 in reverse order
> ( yes 255 | csv-to-bin ub ) | cv-cat --input 'no-header;rows=64;cols=64;type=ub' 'count;accumulate=4,,reverse;view=0;null'
> # accumulate images in fixed order
> ( yes 255 | csv-to-bin ub ) | cv-cat --input 'no-header;rows=64;cols=64;type=ub' 'count;accumulate=4,fixed;view=0;null'
> # accumulate images in fixed order (reverse)
> ( yes 255 | csv-to-bin ub ) | cv-cat --input 'no-header;rows=64;cols=64;type=ub' 'count;accumulate=4,fixed,reverse;view=0;null'

For example, if you want to create an image from fixed number of tiles, you could run something like this:

> ( yes 255 | csv-to-bin ub ) | cv-cat --fps 1 --input 'no-header;rows=64;cols=64;type=ub' 'count;accumulate=4,fixed;untile=2,2;view=0;null'

Say, you process images, but would like to view them in the middle of your pipeline in a different way (e.g. increase their brightness, resize, etc).

Now, you can do it with cv-cat tee. For example:

> # make a test image
> ( echo 20170101T000000,64,64,0 | csv-to-bin t,3ui; yes 255 | head -n $(( 64 * 64 )) | csv-to-bin ub ) > image.bin
> # observe that the images viewed in tee are passed unmodified down the main pipeline for further processing
> for i in {1..100}; do cat image.bin; done | cv-cat --fps 1 "count;tee=invert|view;resize=2" | cv-cat "view;null"

You could specify (almost) any pipeline in your tee filter, but viewing and, perhaps, saving intermediate images in files seem so far the main use cases.

Recently, we found that cv-cat view stopped working properly, when used several times in the same cv-cat call.

Something like

> cat images.bin | cv-cat "view;invert;view;null"

would either crash or behave in undefined way. All our debugging has pointed to some sort of race condition in the underlying cv::imshow() call or deeper in X-windows-related stuff, thus, at the moment, it seems to be out of our control.

Use the following instead:

> cat images.bin | cv-cat "view;invert" | cv-cat "view;null"

cv-cat is now able to perform pixel clustering by color using the k-means algorithm.

for example:

> cv-cat --file rippa.png "convert-to=f,0.0039;kmeans=4;view;null" --stay

input image:

output image (4 clusters):

A new convenience utility ros-from-csv is now available in snark. It reads CSV records and converts them into ROS messages with the usual conveniences of csv streams (customised fields, binary format, stream buffering/flushing, etc).

Disclaimer: ros-from-csv is a python application and therefore may not perform well streams that require high bandwidth or low latency.

You could try it out, using the ROS tutorial Understanding Topics (http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics):

Run ROS Tutorial nodes:

> # in a new shell
> roscore
> # in a new shell
> rosrun turtlesim turtle_teleop_key

 

Send your own messages on the topic, using ros-from-csv:

> echo 1,2,3,4,5,6 | ros-from-csv /turtle1/cmd_vel

Or do a dry run:

> echo 1,2,3,4,5,6 | ros-from-csv /turtle1/cmd_vel --dry
linear: 
  x: 1.0
  y: 2.0
  z: 3.0
angular: 
  x: 4.0
  y: 5.0
  z: 6.0

You also can explicitly specify message type:

> # dry run
> echo 1,2,3 | ros-from-csv --type geometry_msgs.msg.Point --dry
x: 1.0
y: 2.0
z: 3.0
 
> # send to a topic
> echo 1,2,3 | ros-from-csv --type geometry_msgs.msg.Point some-topic

A new convenience utility ros-to-csv is now available in snark. It allows to output as CSV the ROS messages from rosbags or from topics published online.

You could try it out, using the ROS tutorial Understanding Topics (http://wiki.ros.org/ROS/Tutorials/UnderstandingTopics):

Run ROS Tutorial nodes:

> # in a new shell
> roscore
> # in a new shell
> rosrun turtlesim turtlesim_node
> # in a new shell
> rosrun turtlesim turtle_teleop_key

Run ros-to-csv; then In the shell where you run turtle_teleop_key, press arrow keys to observe something like:

> # in a new shell
> ros-to-csv /turtle1/cmd_vel --verbose
ros-to-csv: listening to topic '/turtle1/cmd_vel'...
0,0,0,0,0,-2
0,0,0,0,0,2
-2,0,0,0,0,0
-2,0,0,0,0,0
0,0,0,0,0,2
0,0,0,0,0,-2
2,0,0,0,0,0
0,0,0,0,0,2

If you log some data in a rosbag:

> # in a new shell
> rosbag record /turtle1/cmd_vel

You could convert it to csv with a command like:

> ros-to-csv /turtle1/cmd_vel --bag 2017-11-06-14-43-34.bag
2,0,0,0,0,0
0,0,0,0,0,-2
-2,0,0,0,0,0
0,0,0,0,0,2
0,0,0,0,0,2
0,0,0,0,0,-2
2,0,0,0,0,0

Sometimes, you have a large file or input stream that is mostly sorted, which you would like to fully sort (e.g. in ascending order).

More formally, suppose, you know that for any record Rn in your stream and any records Rm such that m - n > N, Rn < Rm, where N is constant.

Now, you can sort such a stream, using csv-sort, --sliding-window=<N>:

 

> ( echo 3echo 1; echo 2; echo 5echo 4 ) | csv-sort --sliding-window 3 --fields a
0
1
2
3
> ( echo 4echo 5echo 2echo 1echo 3 ) | csv-sort --sliding-window 3 --fields a --reverse
3
2
1
0

As usual, you can sort by multiple key fields (e.g. csv-sort --sliding-window=10 --fields=a,b,c), sort block by block (e.g. csv-sort --sliding-window=10 --fields=t,block), etc.

Sometimes, you have a large file or input stream that is mostly sorted by some fields with just a few records out of order now and then. You may not care about those few outliers, all you want is most of your data sorted.

Now, you can discard the records out of order, using csv-sort, e.g:

> ( echo 0; echo 1; echo 2; echo 1; echo 3 ) | csv-sort --discard-out-of-order --fields a
0
1
2
3
> ( echo 3; echo 2; echo 1; echo 2; echo 0 ) | csv-sort --discard-out-of-order --fields a --reverse
3
2
1
0

As usual, you can sort by multiple key fields (e.g. csv-sort --discard-out-of-order --fields=a,b,c), sort block by block (e.g. csv-sort --discard-out-of-order --fields=t,block), etc.