What the difference of NavSatFix on 2D and 3D?

[elgarbe]

I'm wondering why NavSatFix is already implemented on 3D and not for 2D? Is there a plan to add that msg for 2D?

Thank!

[SirVer]

I was implemented by us here at Lyft because we required it for our work. We only use 3D, so we did not implement it for 2D. It is conceptually easy to port it to 2D, just follow the 3D implementation and selective copy and past concepts back. Lyft has no immediate plans to port this to 3D, but I am sure the 2D port would be greatly appreciated.

/cc @wohe @danielsievers

[elgarbe]

Can you point me out to repro/branch to take a look?

[danielsievers]

Some pointers: https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md

There are various "Not implemented yet" strings in for example

• PoseGraph2d::Add/GetFixedFramePoseData()
• it also needs to be passed to the OptimizationProblem, see for example PR #476 and #477

[ojura]

Hey guys, just wanted to let you know that I am planning to (try to!) upstream my reimplementation of NavSatFix using Landmarks (more precisely, using a single landmark located in the origin, which "observes" the tracking frame). I consider this implementation superior over the current one because it interpolates between trajectory nodes instead of interpolating between fixed frame measurements. Can't exactly make any promises on the timeframe, though ;)

[wohe]

@ojura As discussed before this not superior if you have a INS with a high frequency of updates, e.g. 100 Hz of NavSatFix and only 10 Hz of trajectory nodes. Your solution would increase the amount of constraints added by 10x, right? Also, the current implementation makes use of the fact, that NavSatFix provides an idea of gravity orientation by only rotating the fixed frame around the z axis, i.e. yaw. Did you also make sure that this is maintained by your implementation?

[ojura]

The amount of constraints can be controlled with a sampling factor, just like with the current implementation, so you can adjust this to avoid bloating the pose graph. This approach can thus handle both high and low frequency GPS observations. We can certainly put more robust filtering in there - when accurate GPS observations are dense, we can use them sparsely, and when they become sparser, we can reduce sampling.

Also, the second benefit is that there is no need for explicit gravity/yaw estimation for the fixed frame, you implicitly get it for free because trajectories are already gravity aligned, just like the local reference frame. Let me explain. Cartographer's global map frame is made to be exactly the local GPS reference frame, with the normal being the ECEF radius vector of the first fix. All trajectories are made to be free floating (i.e. the first trajectory is not special anymore and not tied to the origin in any way, which I consider as another benefit). Thus the constraints originating from GPS measurements are effectively constraints from trajectory nodes to the map origin - which I find truer to the name "fixed frame pose (position) measurements". If you set the rotation weights of these constraints to zero, the optimizer is able to orient the trajectories, i.e. find a heading/yaw, in a way which is consistent with GPS measurements.

[Conhokis]

Hey, I've been trying to add navsatfix to 2D, been working around the OptimizationProblem (selective copy paste) and looking for someone or someway to validate the code, especially the part that adds fixed frame constraints. Would greatly appreciate some feedback. optimization_problem_2d.cc

[ojura]

@Conhokis and everyone else who might be interested, my implementation of GPS constraints using the existing landmarks API as described above is available at https://github.com/larics/cartographer/tree/gps_landmarks and https://github.com/larics/cartographer_ros/tree/gps_landmarks. It should work for both 2D and 3D (it has been field tested on a robot with a 2D laser). I think it is an upgrade from the existing implementation because it associates the SLAM world origin (the map coordinate system) with the local ENU frame instead with the origin of the first trajectory (which is now able to be moved around by the optimizator in order to get aligned with the GPS measurements). The GPS measurements can also be visualized as vertical slabs if you display the appropriate marker topic in rviz.

I did not have the time to clean it all up for upstream, but I am putting it here in hope it will be useful for someone.

[Conhokis]

Thank you a lot for this implementation, will be a huge help.

Get Outlook for Androidhttps://aka.ms/ghei36

---

From: Juraj Oršulić notifications@github.com Sent: Wednesday, June 12, 2019 8:45:15 AM To: googlecartographer/cartographer_ros Cc: Conhokis; Mention Subject: Re: [googlecartographer/cartographer_ros] What the difference of NavSatFix on 2D and 3D? (#835)

@Conhokishttps://github.com/Conhokis and everyone else who might be interested, my implementation of GPS constraints using the existing landmarks API as described above is available at https://github.com/larics/cartographer/tree/gps_landmarks and https://github.com/larics/cartographer_ros/tree/gps_landmarks. It should work for both 2D and 3D. I think it is an upgrade from the existing implementation because it associates the SLAM world origin (the map coordinate system) with the local ENU frame instead with the origin of the first trajectory (which is now able to be moved around by the optimizator in order to get aligned with the GPS measurements). The GPS measurements can also be visualized as vertical slabs if you display the appropriate marker topic in rviz.

I did not have the time to clean it all up for upstream, but I am putting it here in hope it will be useful for someone.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/googlecartographer/cartographer_ros/issues/835?email_source=notifications&email_token=ACEVK4K37QFESL7SIANWYADP2CSQXA5CNFSM4E3VW7XKYY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGODXPRKLA#issuecomment-501159212, or mute the threadhttps://github.com/notifications/unsubscribe-auth/ACEVK4M4ROFVQYTDLB26EELP2CSQXANCNFSM4E3VW7XA.

[FeynH]

@ojura Hey, I also want to add navsatfix to 2D. But I don't know how to do on the basis of your work.

[ojura]

@FeynH It should already be working if you enable use_nav_sat_fix and give it some ROS nav sat fix measurement messages. Sorry, due to time constraints I cannot provide detailed step by step support; you will have to take a look at the commits and try it out for yourself. If you have a quick question about something, I can try answering.

[BillYJT]

Hi, @ojura . Thank you for the 2D implementation. Our group is also trying to integrate NavSat data in our 2D SLAM. We installed the cartographer from the github link you provided above. However, on launch the error still occurs "pose_graph_2d.cc: Not yet implemented for 2D".

We suspect that we may not have set up the config.lua file correctly, which we set up as following.

`include "map_builder.lua" include "trajectory_builder.lua"

options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, use_pose_extrapolator = true, use_odometry = false, use_nav_sat = true, use_landmarks = false, num_laser_scans =1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 10, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., }

MAP_BUILDER.use_trajectory_builder_2d = true TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10

return options`

Any insights on what might have gone wrong here? Really appreciate your input. Thanks

[ojura]

@BillYJT please recheck that you have checked out the correct branch for both cartographer and cartographer_ros, gps_landmarks.

If the branch is indeed correct, please post a stack trace.

[BillYJT]

@ojura Thank you for your reply. Turned out we indeed used the wrong branch. But after the correct branch is checked out, i.e. gps_landmarks, we encountered the following error on launch.

F0722 14:00:08.000000 22828 lua_parameter_dictionary.cc:399] Check failed: HasKey(key) Key 'nav_sat_predefined_enu_frame_lat_deg' not in dictionary:

We tried to include the mentioned parameter 'nav_sat_predefined_enu_frame_lat_deg' in the lua configuration file, but other missing parameters came up in the error.

We suspect that the error arises because an old 'lua_parameter_dictionary.cc' is called, due to that we launch from two workspaces, one that only contains the cartographer(gpa_landmarks) and the other that contains the customized packages.

We haven't yet found a way to resolve this.

[ojura]

Add all missing nav_sat_predefined_enu_frame* parameters. If you do not use a predefined enu frame, the first gps measurement will be used for creating one.

[BillYJT]

@ojura Thank you for your help. We are now able to launch without error.

However, we get a warning as follow. [ WARN] [1563779342.214382600]: W0722 17:09:02.000000 31568 ordered_multi_queue.cc:155] Queue waiting for data: (0, fix)

The rostopic /fix does not appears to be running, if even though the use_nav_sat param has been set to true.

Running following gives: ~$ rostopic echo fix WARNING: topic [/fix] does not appear to be published yet

This is part of our launch file that launches cartographer `<node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args=" -configuration_directory $(find ursa)/config -configuration_basename cartographer_config.lua" output="screen">

      <remap from='imu' to='filtered_imu'/>
      <remap from='fix' to='/mavros/global_position/raw/fix'/> 
</node> `

'/mavros/global_position/raw/fix' seems to be publishing proper data when we 'rostopic echo' it.

[RonaldEnsing]

The rostopic /fix does not appears to be running, if even though the use_nav_sat param has been set to true.

You are remapping the topic correctly, however, you should check your remapped topic then: /mavros/global_position/raw/fix instead of /fix.

However, we get a warning as follow. [ WARN] [1563779342.214382600]: W0722 17:09:02.000000 31568 ordered_multi_queue.cc:155] Queue waiting for data: (0, fix)

I am also trying to use the gps_landmarks branch and I am running into this issue as well. This warning shows up after a minute. The cartographer_node is successfully subscribed to the fix topic. The map-to-odom tf does not get published at all.

[BillYJT]

@RonaldEnsing Hi. We have checked /mavros/global_position/raw/fix, which is publishing properly.

Our problem seems to be that the /fix is not there among the rostopics at the moment, and cartographer has no /fix to subscribe to

[RonaldEnsing]

Our problem seems to be that the /fix is not there among the rostopics at the moment, and cartographer has no /fix to subscribe to

Since you are remapping the topic, it makes sense that /fix does not show up when you do rostopic list. Cartographer is probably subscribing successfully to your /mavros/global_position/raw/fix topic. You can check this with e.g. rqt_graph.

I still don't know why the Queue waiting for data: (0, fix) shows up. In my case, I am subscribing to NavSatFix messages with a good quality (NavSatStatus is set accordingly). However, the GPS data never seems to up in the TrajectoryBuilder, and I am not sure if it should end up in the TrajectoryBuilder.

This statement [1] always evaluates to true (no data). The time stamps of the scan, imu and fix messages are close to each other. The tf tree between base_link and my sensors frame_id's are setup correctly. So I do not suspect an issue regarding this.

If the gps_landmarks branch works for other people, then I suspect something may be wrong with my configuration:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,

  use_odometry = false,
  use_nav_sat = true,
  nav_sat_translation_weight = 1.0,
  nav_sat_use_predefined_enu_frame = false,
  nav_sat_predefined_enu_frame_lat_deg = 51.99993,
  nav_sat_predefined_enu_frame_lon_deg = 4.36988,
  nav_sat_predefined_enu_frame_alt_m = 0,

  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,

  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 0.5,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 0.1,
}

MAP_BUILDER.use_trajectory_builder_2d = true

return options

@ojura Any idea what could be wrong? Would you be able to share your configuration?

[1] https://github.com/larics/cartographer/blob/gps_landmarks/cartographer/sensor/internal/ordered_multi_queue.cc#L99

[ojura]

@RonaldEnsing can you share your bag and the full invocation line with all remappings for the offline node?

[BillYJT]

@RonaldEnsing Thank you.

You are right, after examing on the rqt_graph, cartographer_node seems to subscribe to /mavros/global_position/raw/fix successfully.

Looks like we encounter the same issue. I.e. when use_nav_sat is enabled in the config, the odom frame becomes missing entirely, and map and base_link end up in different tf trees.

@ojura Also following on the solution to this. Thanks a lot.

[RonaldEnsing]

@RonaldEnsing can you share your bag and the full invocation line with all remappings for the offline node?

I used the online node so far, because I have a few other nodes that run at the same time (e.g. a pointcloud_to_laserscan node and a node that adds gravity to my IMU messages). So I have just created the rosbag that can be used with offline cartographer, and I see that I do not have this issue with offline cartographer.

Since github only allows attachments with certain file types (and BAG is not one of the allowed file types), I have attached the rosbag file as a ZIP.

2019-07-24_cartographer_processed_for_offline.zip

This is my invocation with a launch file for offline mode:

<?xml version="1.0"?>
<launch>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
      -configuration_directory $(find prius_launch)/launch/localization/cartographer
          -configuration_basenames prius_2d.lua
          -urdf_filenames $(find prius_description)/urdf/prius.urdf
          -bag_filenames cartographer_processed_for_offline.bag"
      output="screen">

    <remap from="scan" to="/scan" />
    <remap from="imu" to="/imu_transformer"/>
    <remap from="fix" to="/nav_sat_zero_altitude" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" output="screen"/>
</launch>

This is my launch file for online mode:

<launch>
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
      -configuration_directory $(find prius_launch)/launch/localization/cartographer
          -configuration_basename prius_2d.lua
          "
      output="screen">
    <remap from="scan" to="/scan" />
    <remap from="imu" to="/imu_transformer" />
    <remap from="fix" to="/nav_sat_zero_altitude" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" output="screen"/>
</launch>

[Ellon]

Hi,

I'm just starting to use SatNavFix with my own datasets in cartographer in 3D. In the current master branch (92a8b81a8489dd71a73c) it seems that the displacement between the gps frame and the imu frame is not taken into account. Is that the case or I'm missing something? If that's the case, shouldn't it be taken into account? In some vehicles the GPS and the IMU may be positioned in different parts of the vehicle. Wouldn't that be a problem?

Sorry to jump into the discussion like this, but this thread is quite hot and I think the question is related with the discussion. :)

[ojura]

@RonaldEnsing georeferencing works for you with the online node? Thanks for uploading the bag, I will take a look.

@Ellon you are indeed correct, we do not take into account the arm length between the tracking frame and the antenna. The way I implemented gps using landmarks was by adding landmark observations where the landmark observes the robot, instead of the usual when it’s the other way around. I then introduced a fixed landmark at the origin and had it observe the robot at gps positions.

This could be improved in the landmark residual function to have the landmarks observe a part of the robot, ie. the antenna, with a known relative pose to the tracking frame. I am planning to add that at one point, I can’t promise when.

[RonaldEnsing]

@RonaldEnsing georeferencing works for you with the online node? Thanks for uploading the bag, I will take a look.

Both online and offline mode do not work. In addition, the online mode does not produce a map frame.

edit: In addition, in this line (https://github.com/larics/cartographer/blob/gps_landmarks/cartographer/sensor/internal/ordered_multi_queue.cc#L99), data is always nullptr for my 'fix' data.

[ojura]

@RonaldEnsing I have tested your bag and it looks like gps alignment works. It looks like there is a bug when collating gps fixes with other sensors is enabled, for now you can disable it until I push a fix. Thanks for reporting.

If you visualize the landmark poses topic, you can see that the vertical green slabs are there, which display the used GPS positions.

I've had problems tuning local SLAM, it messes up the beginning, but it recovers enough to build that straight part of the trajectory on the picture.

This is the tuning I have used:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,

  use_odometry = false,
  use_nav_sat = true,
  nav_sat_translation_weight = 1e4,
  nav_sat_use_predefined_enu_frame = false,
  nav_sat_predefined_enu_frame_lat_deg = 51.99993,
  nav_sat_predefined_enu_frame_lon_deg = 4.36988,
  nav_sat_predefined_enu_frame_alt_m = 0,

  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,

  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(15.)
TRAJECTORY_BUILDER.collate_fixed_frame = false

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.1
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 5

return options

And the invocation:

rosrun cartographer_ros cartographer_offline_node -configuration_directory . -configuration_basenames prius_2d.lua -use_bag_transforms=true -bag_filenames 2019-07-24_cartographer_processed_for_offline.bag imu:=imu_transformer fix:=nav_sat_zero_altitude -keep_running=true

[RonaldEnsing]

Thank you @ojura . I am able to reproduce this.

It looks like there is a bug when collating gps fixes with other sensors is enabled, for now you can disable it until I push a fix.

What other sensors are you referring to and what needs to be disabled?

[ojura]

Cartographer is using a multi-ordered sensor queue to sort (collate) all incoming sensor data. Ie. lidar, IMU, odometry, gps. Optionally, gps measurements can skip this sensor collation and be directly injected into the pose graph whenever they arrive by setting: TRAJECTORY_BUILDER.collate_fixed_frame = false

It looks like I messed up in this implementation the default case when collating gps measurements is enabled (set to true). The difference is negligible, it makes the process slightly nondeterministic, but I cannot promise when I will get around to fixing that.

[RonaldEnsing]

Thanks for the explanation @ojura.

Would this mean that a map (pbstream file) with use_nav_sat is geo-referenced, meaning that the saved pbstream file can be related with other types of geographic information (e.g. traffic rules)? A geo-referenced map would be beneficial for multi-trajectory SLAM and initialization of the robot pose using the robots GPS device.

edit: According to https://github.com/googlecartographer/cartographer_ros/issues/869 it's not geo-referenced. If a predefined geographic location is used in the lua file (nav_sat_use_predefined_enu_frame) consistently, I think this will work.

[ojura]

Yes, that would mean it’s georeferenced. In this implementation Cartographer’s map frame becomes associated with an east-north-up (ENU) frame tangential to the wgs84 ellipsoid, shared between all trajectories. You can specify the tangent point using the predefined enu frame option, and if not, the first gps measurement is used as the tangent point. It makes sense to consistently use one enu frame for multiple trajectories.

[RonaldEnsing]

Are you sure that the map frame that cartographer provides is an ENU orientation? It seems to be a south-east-up orientation if you look at these pictures I attached, based on the bag file I provided in an earlier comment in this issue.

In this first picture north is up. The trajectory was driven counter clockwise, from the green point to the red point. As you can see the largest part of the trajectory was driven in the NNW direction. This picture is as expected.

The second picture is a visualization at the end of the trajectory in rviz running cartographer. The trajectory is visualized in blue. The x-axis (red) of the map frame points to the right. That would be the south direction if you compare this with the first picture.

edit: I just found out that according to [1], it is not necessarily an ENU frame, since rfcs 0007 states: "Then we will transform to a local ground reference frame (e.g. ENU) with the z-axis pointing up, and the origin at the start position." Also this commit message [2] does not state ENU explicitly, only a local frame with the z-axis pointing up.

A minus 90 degree rotation around Z of the local frame seems to create an ENU frame. I changed this [3] line into return cartographer::transform::Rigid3d::Rotation(Eigen::AngleAxisd((-0.5 * M_PI), Eigen::Vector3d::UnitZ())) * cartographer::transform::Rigid3d(rotation * -translation, rotation); to create an ENU frame, making the map frame ENU and conforming to REP 103 [4].

[1] https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md [2] https://github.com/googlecartographer/cartographer_ros/commit/b0a937b7ed6a910193b1be534d179196f5f9480d [3] https://github.com/larics/cartographer_ros/blob/e064e9c68a560339abed92713294bde946db3917/cartographer_ros/cartographer_ros/msg_conversion.cc#L370 [4] https://www.ros.org/reps/rep-0103.html

[ojura]

The behaviour has been changed from the description in rfcs. Previously, the map frame was always at the beginning pose of trajectory 0. In the gps_landmarks branch, if you enable gps, trajectory 0 is completely uncoupled from the map origin and is aligned to pass through gps position measurements.

Are the gps position measurement green lines visible (the landmark poses topic)? What is their relationship with the map frame? It should be ENU. And it should not depend on SLAM results in any way. And since the trajectory is optimized to pass through them, the map frame should also be ENU with respect to trajectory 0.

If the green gps position markers are not corresponding to an ENU map frame, that means I have messed something up, please do let me know.

[RonaldEnsing]

The picture below shows the gps position measurements (/landmark_poses_list) in green. This does not look like an ENU map frame.

[ojura]

@RonaldEnsing would you mind sharing the data and the tuning again?

[RonaldEnsing]

This is the data: https://github.com/googlecartographer/cartographer_ros/files/3426153/2019-07-24_cartographer_processed_for_offline.zip. It's still the same data as here https://github.com/googlecartographer/cartographer_ros/issues/835#issuecomment-514571436.

Regarding the tuning I've used the nav_sat_use_predefined_enu_frame option. The location of the map frame is as expected, only the orientation is not what I expect for ENU.

  use_nav_sat = true,
  nav_sat_use_predefined_enu_frame = true,
  nav_sat_predefined_enu_frame_lat_deg = 51.999915,
  nav_sat_predefined_enu_frame_lon_deg = 4.370100,
  nav_sat_predefined_enu_frame_alt_m = 0

[ojura]

Thanks. Could you please paste the whole tuning in case it's different from above? It looks like you managed to improve local slam not to mess up the beginning.

[RonaldEnsing]

This is my complete lua file:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = true,

  use_odometry = false,
  use_nav_sat = true,
  nav_sat_translation_weight = 1.0e40, --very large weight on nav sat for #835
  nav_sat_use_predefined_enu_frame = true,
  nav_sat_predefined_enu_frame_lat_deg = 51.999915,
  nav_sat_predefined_enu_frame_lon_deg = 4.370100,
  nav_sat_predefined_enu_frame_alt_m = 0,

  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,

  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 0.1,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 0.1,
}

MAP_BUILDER.use_trajectory_builder_2d = true

MAP_BUILDER.num_background_threads = 12
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(10.)
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 1.
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.2
POSE_GRAPH.constraint_builder.log_matches = true
POSE_GRAPH.constraint_builder.max_constraint_distance = 5
POSE_GRAPH.constraint_builder.min_score = 0.2
POSE_GRAPH.constraint_builder.sampling_ratio = 1
POSE_GRAPH.global_sampling_ratio = 0.03
POSE_GRAPH.max_num_final_iterations = 1e2
POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 1e-2
POSE_GRAPH.optimization_problem.fixed_frame_pose_translation_weight = 1e10
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight    = 1e-10
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e-10
POSE_GRAPH.optimization_problem.log_solver_summary = true
POSE_GRAPH.optimization_problem.odometry_rotation_weight           = 1e10
POSE_GRAPH.optimization_problem.odometry_translation_weight        = 1e10
POSE_GRAPH.optimize_every_n_nodes = 10
TRAJECTORY_BUILDER.collate_fixed_frame = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 12
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.max_range = 200.
TRAJECTORY_BUILDER_2D.min_range = 0.45
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 30.
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.5
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(15.)
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 1.0
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.3
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 1
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.6
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.4
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.5

return options

[RonaldEnsing]

@ojura Do you agree that the 'map' frame is not an ENU frame? Were you able to reproduce this with my configuration?

[ojura]

Sorry, I was a bit swamped last month, didn't get the time to look into it yet. But don't worry, I definitely will.

[ojura]

@RonaldEnsing I had a look. Look like we have a south-east-up frame instead of east-north-up?

BTW submap size of 1 scan really does not make sense. Something like 25 makes more sense.

[ojura]

@RonaldEnsing I have pushed to larics/cartographer_ros, branch gps_landmarks. The local frame should now be ENU.

[donrv]

@ojura I got below error when launched in 3d: Am I missing something? I am using gps_landmarks branch of cartographer and cartographer_ros My config file:

[FATAL] [1572617463.027501002]: F1101 19:41:03.000000 28911 lua_parameter_dictionary.cc:399] Check failed: HasKey(key) Key 'tsdf_range_data_inserter' not in dictionary: { probability_grid_range_data_inserter = { hit_probability = 0.550000, insert_free_space = true, miss_probability = 0.490000, }, range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", }

[ojura]

@donrv check that you have checked out the appropriate branch for both cartographer and cartographer_ros. Also be sure you don’t have a third source of incompatible (older version) configuration files, e.g. system installed cartographer.

[donrv]

Did a clean build, It's working. Thank you so much

[MarcelSoler]

Hi! I am trying to use 2D slam with Navsat fix, I found out that not implemented yet on Cartographer, (correct me if I am wrong please). Does anybody know if that will be implemented soon? Also, https://github.com/larics/cartographer/tree/gps_landmarks can be merged to official cartographer @ojura ?

[ojura]

Perhaps the code quality of that is a bit hacky for it to be merged as-is. However, Cartographer's development is unfortunately suspended, AFAIK.

[wohe]

cartographer-project/cartographer#1580 got merged.

[darthShana]

Hi @ojura Im attempting to use your branch to enhance google cartographer with gps. With the hope it can continue to track my robot when the lidar cant find features in range.. However it doesnt appear to be working.. I have checked out your branch and have run the robot Here is a video of whats happening. As you can see the robot correctly tracks its position while lidar features are available but then stops when it move out of range (even though the robot continues on a straight line) Im not seeing any errors and i do see "green lines" that look correct, which makes me think that the GPS fix is being received correctly. However if doesnt seem to help with the robot transformation.

Any help would be greatly appreciated. bellow is my config

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
 map_builder = MAP_BUILDER,
 trajectory_builder = TRAJECTORY_BUILDER,
 map_frame = "map",
 tracking_frame = "imu_link",
 published_frame = "base_link",
 odom_frame = "odom",
 provide_odom_frame = true,
 publish_frame_projected_to_2d = false,
 use_odometry = false,
 use_nav_sat = true,
 use_landmarks = false,
 num_laser_scans = 1,
 num_multi_echo_laser_scans = 0,
 num_subdivisions_per_laser_scan = 1,
 num_point_clouds = 0,
 lookup_transform_timeout_sec = 0.2,
 submap_publish_period_sec = 0.3,
 pose_publish_period_sec = 5e-3,
 trajectory_publish_period_sec = 30e-3,
 rangefinder_sampling_ratio = 1.,
 odometry_sampling_ratio = 1.,
 fixed_frame_pose_sampling_ratio = 1.,
 imu_sampling_ratio = 1.,
 landmarks_sampling_ratio = 1.,
 nav_sat_translation_weight = 1e4,
 nav_sat_use_predefined_enu_frame = false,
 nav_sat_predefined_enu_frame_lat_deg = 36.90744,
 nav_sat_predefined_enu_frame_lon_deg = 174.79056,
 nav_sat_predefined_enu_frame_alt_m = 0,

}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 12.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
TRAJECTORY_BUILDER.collate_fixed_frame = false

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 15
POSE_GRAPH.constraint_builder.min_score = 0.65

return options