Pure localization

[Damien R]

Hi,

AFAIK in pure localization cartographer starts a trajectory and it is not "globally" localized until it founds a constraint with the map. I would like to know if it is possible to add an initial constraint between the trajectory and the map to have a faster localization.

I see that since #482 it is possible to add a fixed frame pose (for now only in 3D).

• Is it the recommended way to add the initial constraint or something else should be done (for e.g: setting a initial pose for the submap)? 
  
• Can you give me hint to add the support for fixed frame pose in 2D especially for optimization_problem.cc as I think the rest is the same as 3D?

     if (!fixed_frame_pose_initialized) {

        const transform::Rigid3d fixed_frame_pose_in_map =

            node_data[node_index].point_cloud_pose *

            constraint_pose.zbar_ij.inverse(); // TODO maybe change this

        C_fixed_frames.emplace_back(

            transform::Rigid3d(

                fixed_frame_pose_in_map.translation(),

                Eigen::AngleAxisd(

                    transform::GetYaw(fixed_frame_pose_in_map.rotation()),

                    Eigen::Vector3d::UnitZ())),

            nullptr,

            common::make_unique<ceres::AutoDiffLocalParameterization<

                YawOnlyQuaternionPlus, 4, 1>>(), // TODO maybe change this

            &problem);

        fixed_frame_pose_initialized = true;

      }  

      problem.AddResidualBlock(

          new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(  // TODO maybe change this

              new SpaCostFunction(constraint_pose)),

          nullptr, C_fixed_frames.back().rotation(),

          C_fixed_frames.back().translation(),

          C_nodes.at(trajectory_id).at(node_index).rotation(),

          C_nodes.at(trajectory_id).at(node_index).translation());

One more question, for now the map is never updated but do you know what should be done to support this feature?

Regards,

Damien R.

[Akshay Roy]

I am also interested in knowing this. As of now, I am giving my initial pose to cartographer via rviz similar to what we do in amcl and  I have used the following strategy to localize:

1.Need to writer a subscriber in node.cc that would subscribe to /initialpose from rviz.

2.Whenever we receive an /initialpose we need to first finish the current Trajectory and then run final optimization.

3.Then a new trajectory needs to be started from the pose given by /initialpose and need to make sure it is with respect to map frame.

4.Push this pose all the way back to LocalTrajectoryBuilder class where the PoseEstimate is defined and change the value from identity to this pose.

5.Then pass the current pose estimate from localtrajectorybuilder to the sparse_pose_graph and then constraint_builder and finally to fast correlative scan matcher (MatchFullSubmap)

6.And instead of starting to scan match from the origin(or centre) of the submap, scan match from the current best pose estimate.(only when robot is in or near that submap)

As of now the cartographer tries to find a scan match in all trajectories from submap 0 to the last submap. For a large map with say 100 submaps  and if we start somewhere near 100th submap it might take a long time for the robot to localize. So I am continuously calculating the distance between the pose of the robot(after you give an initial pose which also might have some error) and the origin of the submap in global frame(map frame) and finding the nearest submap in all trajectories and I scan match against that submap first, if we can connect trajectories then good enough else it will again try to scan match against all the submaps in all trajectories again as before. 

I have tested with their bag file and it does localize faster as far as i can see(visually in rviz) with the same configuration. But still haven't done proper testing.

Some Issues:

I am finding it difficult to delete a trajectory, since it is not correct. As of now I have used the following strategy to delete the submaps:

1.I have added another instance of trimmer that would trim sparse pose graph(with the submaps to keep as 0)

2.Then when i finish trajectory set the state of all the submaps in that trajectory as finished.

3.Then call the new trimmer instead of the other pre-existing one only when initial pose is added.(In RunOptimzation).

This was just an initial quick fix that I used, definitely not the efficient way, would love know about better methods to do this.

But I am getting errors whenever I give an initial pose. It is quite difficult to track down where all the submaps ids and trajectories ids go. Is there any efficient way to say just delete the trajectory and all the submaps in it if we know it is wrong, ezpecially during localization were we dont care building a map. 

Some improvements:

I think its better to sort the submap_ids in each trajectory based on the pose of the robot(especially when the are embedded with intial pose as in this case). In that case we can start to scan match against the nearest submap and then move sequentially away from that.(working on this now)

[Damien R]

I see that you are busy on some refactoring and improvements for 3D, but can you give me a vague picture of what should be done to support faster initialization of new trajectory.

For e.g. let's take this use case:

- detection of a special beacon (GPS, QRCode, ...)

- start of a new trajectory with the pose of the special beacon

Regards,

Damien R.

[Akshay Roy]

On Friday, September 1, 2017 at 2:36:55 AM UTC-4, Damien R wrote:

I see that you are busy on some refactoring and improvements for 3D, but can you give me a vague picture of what should be done to support faster initialization of new trajectory.

I am actually working on 2D SLAM.  

For e.g. let's take this use case:

- detection of a special beacon (GPS, QRCode, ...)

- start of a new trajectory with the pose of the special beacon

The procedure would be the same as I mentioned above. You need to subscribe to an initial pose(like in your case GPS or QRCode) and send this initial pose all the way back to fast correlative scan matcher where the search radius starts from the center of the submap(make sure you make the changes for global scan matching case) to the pose estimate given by you. You can also reduce the search radius if you are very confident with your pose estimate.

 

Regards,

Damien R.

Regards,

Akshay