Note: For this tutorial you need a working and calibrated camera using the ATAN camera model. This tutorial assumes that you have completed the previous tutorials: camera_calibration, using_ethzasl_ptam.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Tutorial for using ethzasl_ptam remotely

Description: This tutorial explains how to work with and use ethzasl_ptam remotely in case the host (i.e. robot) does not have any display posibilities.

Tutorial Level: INTERMEDIATE

Next Tutorial: ethzasl_sensor_fusion: getting_started

Preparation

This tutorial assumes that you have a working camera streaming monochrome images on a ROS image topic. Also, it assumes you to have stored correct camera calibration parameters and image resolution parameters in PtamFixParams.yaml. Cameras with rolling shutter do work to some extent, however, feature extraction and matching performance deteriorates greatly with more motion. When working with ethzasl_ptam, ensure that there is sufficient light. As a rule of thumb, the camera shutter speed should always be lower than 5ms to avoid motion blur. Furthermore, try to avoid rotations whenever possible, feature triangulations yield best results after sufficient and pure translations.

For this tutorial, we disable the gui of PTAM by setting "gui: False" in PtamFixParams.yaml. Save the file and launch ethzasl_ptam and the camera streaming.

If you have no camera at hand, you may download this dataset (151MB).

Using the Remote Interface

ethzasl_ptam has a node called remote_ptam to remote control the ptam node:

rosrun ptam remote_ptam

a window named vslam/preview showing the current camera feed should appear. This window acts similar to the PTAM window learned in the previous tutorial. The 3D view is not available in this window and the tracked features are not painted. However, the map grid is displayed and all key-board interactions are the same as in the PTAM window discussed before.

The transferred image is the first pyramidal level of the original camera image in order to not overload the channel.

Remote Visualization

Since remote_ptam is limited in displaying all information, we wrote the ptam_visualizer node which transfers 3D features, key-frames and camera path to rViz.

First, initialize a map, then, start ptam_visualizer

rosrun ptam ptam_visualizer

In the dynamic reconfigure gui, select /ptam_visualizer. Also, start rViz.

Displaying the 3D features

In the dynamic reconfigure gui enable "ShowPC". In rViz add a PointCloud2 topic and map it to /vslam/pc2. Select "Billboard Spheres as display style, augment the decay time to, say, 10000 sec (use 0 if you only want to display the latest reading) and select "RGB8" as color transformer. You should now see a 3D point cloud colored according to the feature's pyramidal level.

Displaying the Key-Frames

In the dynamic reconfigure gui enable "ShowKFs". In rViz add a MarkerArray topic and map it to /vslam/kf_visualization_array. You should now see all key-frames retained in the current map. The amount of the key-frames displayed is defined in the ethzasl_ptam parameter "MaxKF". If "KFFlags" in the dynamic reconfigure gui is set to a negative value -N, only the last N key-frames are displayed regardless of the "MaxKF" value. "KFFlags"=0 displays all "MaxKF" key-frames and "KFFlags"=1 only displays the last key-frame.

Per default, the marker arrays (i.e. the key-frames) have a lifetime of 1 second. To augment this time, increase "KFLifetime" in the reconfigure gui.

To show all generated key-frames since the last map reset, enable "ShowAllKFs" in the reconfigure gui. Key-frames beyond "MaxKF" are not used anymore in ethzasl_ptam, they are only stored for visualization purpose. This may be used for visualizing a drift when returning to the same position while retaining only a few key-frames in the map. A similar effect has a high value in "KFLifetime", however, then, the keyframes cannot be hidden easily anymore.

Displaying the Trajectory

Sometimes, showing all key-frames may overload the visualization area. For better visibility, you can only show the trajectory by enabling "ShowPath" in the dynamic reconfigure gui. In rViz add a Marker topic and map it to /vslam/path_visualization. You should now see a line in 3D showing the path made by the camera.

To vary the displayed path length, change the value in "PathLength" in the dynamic reconfigure gui. Every second, the current camera pose is transmitted. Thus, retaining 10 path segments is equivalent to the path of the last 10 seconds.

remote_ptam_rviz.jpg 3D point-cloud of a keyboard displayed together with the retained key-frames and the trajectory.

Storing the Data

ptam_visualizer offers also the option to store data in text files for further offline processing.

In the dynamic reconfigure gui in the field "ExportPrefix" you can enter a file prefix to save the data. The file name is concatenated as follows

"~/" + ExportPrefix + TimeNow + Postfix

clicking "ExportPC" will only export the 3D features (the file has the Postfix="_PC") whereas "SaveMap" exports the 3D features (Postfix="_PC"), the key-frames (Postfix="_KF") and the path (Postfix="_Path"). All data is recorded at 1Hz and only the data currently used in ethzasl_ptam is exported (no key-frame or feature history etc). If you want to record high frequent camera poses, we suggest to record directly the published camera poses and the corresponding user messages

rosbag record /vslam/pose /vslam/info

File Format

The 3D feature storage files have the following columns:

[feature x]

[feature y]

[feature z]

[color code]

[pyramidal level]

[corresponding KF number]

The key-frame storage files have the following columns:

[KF number]

[KF x]

[KF y]

[KF z]

[KF qw]

[KF qx]

[KF qy]

[KF qz]

where q is the attitude quaternion of the key-frame.

The path storage files have the following columns:

[path x]

[path y]

[path z]

Wiki: ethzasl_ptam/Tutorials/remote_ptam (last edited 2012-09-21 16:53:27 by stephanweiss)