Start a new map with initialpose

[jamestkpoon]

Hi,

I would like to create a new map, with a camera that starts at a specified pose. So far my code follows:

SLAM->startup();
SLAM->enable_mapping_module();
SLAM->relocalize_by_pose_2d(M, normal_vector);

where M is some pre-defined affine transform matrix and normal_vector is the same as in init_pose_callback().
This order seemed intuitive from following run_localization.cc, however the relocalize_by_pose_2d(...) does not seem to have an effect despite returning true, and so the mapping starts off with the sensor at identity. All of these happen before the node receives its first set of image data.

For illustration, below is the path taken by a camera mounted on a mobile platform driving in a small loop. The camera is pitched down ~30 degrees. The desired result is that the root of each arrow is along the horizontal plane visualized by the grid.

Any insights would be greatly appreciated.
Thanks!

[ymd-stella]

Set the frame_id of camera frame to the image topic and define the tf between the camera frame and the base_link, base_link and odom. Then you can use the tf from map to base_link.

[jamestkpoon]

Thanks for the fast reply, however I am still curious as to why I can not simply call relocalize_by_pose() myself at the start.
It would be a very convenient way to be able to establish a "world" origin that is in a position that the camera cannot physically reach such as on the ground plane, so that maps can be consistent for usage with different autonomous platforms. If an initial pose cannot actually be set, instead could it be possible to transform features into a "world" frame prior to saving?

[ymd-stella]

There is no need to move the origin of the map. The purpose can be accomplished by creating a tf that transforms from the world frame to the map frame.

[jamestkpoon]

Yes, I will have to package each map with an additional transform from world to "slam frame". A bit less elegant but workable.
As a final question given what I have observed, is relocalize_by_pose() only used for localization when the mapping module is disabled?

[ymd-stella]

You can use it. It is just not a function to force the camera position.