roscore
rviz
rosbag play --clock yourbagname.bag
rosparam set use_sim_time true
rosrun oort oort_ros scan:=scan_reduced_mapping

{
    # Robot's TF frame, used to relate to odom frame.
    "base_frame": "base_footprint",

    # Odom's TF frame.
    "odom_frame": "odom",

    # Frame of the depth sensor/laser scanner.
    "sensor_frame": "hokuyo_laser_link",

    # Frame of the map.
    "map_frame": "map",

    # Amount of linear translation of base_frame in odom_frame (in meters)
    # before creating a new graph node.
    "linear_update": 0.2,

    # Amount of angular rotation (in radians) of base_frame in odom_frame (in
    # meters) before creating a new graph node.
    "angular_update": 0.6,

    # Point clouds with less points than this will not be used by Oort for
    # mapping.
    "min_pointcloud_size": 3,

    # Scalar to apply to switchable constraint prior's error.
    "switchable_constraint_scale": 2,

    # Scale on prior constraint for switching variable. By default, switching
    # variables are biased to to turn on so larger values make turning off loop
    # closure constraints more expensive, and thus less likely.
    "switchable_constraint_scale": 2,

    # Params controlling what published rendered maps look like.
    "output_map": {

        # Params controlling rendering laser hits to a probability grid.
        # Mostly from https://google-cartographer.readthedocs.io/
        # en/latest/configuration.html#cartographer-mapping-2d-proto-rangedatainserteroptions
        "probability_grid": {
            # Resolution of map to generate in meters.
            "resolution": 0.05,

            # If ‘false’, free space will not change the probabilities in the
            # occupancy grid.
            "insert_free_space": true,

            # Probability change for a hit (this will be converted to odds and
            # therefore must be greater than 0.5).
            "hit_probability": 0.55,

            # Probability change for a miss (this will be converted to odds and
            # therefore must be less than 0.5).
            "miss_probability": 0.49,

            # Whether to interpret inf readings as evidence of freespace.
            "clear_infs": true,

            # Distance in meters to clear when processing inf readings.
            "inf_clear_dist": 0.5
        },

        # Params controlling thresholding of generated probability grid to
        # create ROS OccupancyGrid
        "threshold":
            {
                # Max occupancy probability for a cell to be labeled freespace.
                "max_free": 48,

                # Min occupancy probability for a cell to be labeled occupied
                "min_occupied": 55
            },

        # Footprint of robot in meters to use for clearing OccupancyGrid in
        # spots where the robot traveled. Helpful for keeping doorways &
        # hallways navigable.
        "robot_radius": 0.15,

        # Strategy to use for clearing speckle noise in final rendered map.
        # either "median" or "morphological".
        "speckle_strategy": "median",

        # In the morphological strategy, Oort splits the rendered map into a
        # map of freespace pixels, and another map of occupied pixels and then
        # it runs morphological iterations on each separately. The two maps
        # are then layered, with freespace first and occupied cells last.
        "morphological":
        {
            # Number of morphological iterations to run on freespace layer.
            "free_space_morpho_iterations": 1,

            # Number of morphological iterations to run on occupied layer.
            "occupied_morpho_iterations": 1
        },

        # In the median strategy, Oort runs a median filter.
        "median":
        {
            # Size of kernel in pixels.
            "kernel_size": 3,

            # Number of iterations of median filtering to run.
            "iter": 2
        }
    },

    # Params controlling how links between consecutive poses are formed in
    # graph.
    "incremental_graph_builder":
        {
            # Covariance to use in the x, y, and theta direction when falling
            $ back to using wheel odometry.
            "odom_cov_xx": 0.02,
            "odom_cov_yy": 0.02,
            "odom_cov_tt": 0.01,

            # Which scanmatcher to use.
            # Either "frsm_matcher" or "cartographer_matcher".
            "scanmatcher": "frsm_matcher",

            # Params when using libfrsm's scanmatcher. This scanmatcher tends
            # to perform better when the pointcloud is less dense as it's able
            # to connect points into line segments.
            "frsm_matcher":
                {
                    # Whether to show opencv debug viewer for the scanmatcher
                    # (need to run Oort in environment with X11 available to
                    # work).
                    "debug": false,

                    # Params for the debug viewer.
                    "debug_viewer":
                    {
                        # How much to scale up the debug viewer.
                        "viewer_scale": 4,

                        # Length of pose history to show.
                        "history_length": 30,

                        # Resolution of debug map.
                        "resolution": 0.025,

                        # Params for probability grid used by debug viewer (see
                        # "probability_grid" above).
                        "insert_free_space": false,
                        "hit_probability": 0.55,
                        "miss_probability": 0.49
                    },

                    # Unused.
                    "meters_per_pixel": 0.01,

                    # Unused.
                    "theta_resolution": 0.01,

                    # Max number of scans to keep as history in scanmatcher.
                    # Note: This is fairly important as it controls how much
                    # information the scanmatcher's incremental mini-map
                    # contains.
                    "max_num_scans": 20,

                    # Params from libfrsm (see
                    # https://github.com/abachrach/frsm/blob/master/src/libfrsm/ScanMatcher.hpp#L64)
                    "initial_search_range_xy": 0.01,
                    "initial_search_range_theta": 0.01,
                    "max_search_range_xy": 0.1,
                    "max_search_range_theta": 0.1,
                    "add_scan_hit_thresh": 0.95,
                    "stationary_motion_model": false,

                    # Standard deviation of motion model estimate. Below .1 the
                    # prior isn't used, and otherwise specifies the standard
                    # deviation for a Gaussian around the odometry estimate.
                    # This controls how closely scanmatching will try to keep
                    # to the odometry estimate.
                    "std_mot_model": 0.4,

                    # Amount to scale the covariance returned by the
                    # scanmatcher.
                    "scanmatch_cov_scale": 5,

                    # Amount to scale the covariance by when mapping is paused
                    # and then "resume from paused".
                    "inflate_scanmatch_cov_scale": 40,

                    # Amount to scale covariance by when a new mapping session
                    # is initiated. This type of resume mapping assumes that
                    # mapping was stopped and then restarted with a guess of an
                    # initial location.
                    "resume_scanmatch_cov_scale": 50,

                    # Unused
                    "sm_dist_accept_threshold": 0.06,

                    # Unused.
                    "sm_angle_accept_threshold": 0.07,

                    # Unused.
                    "min_sm_accept_score": 0.45,

                    # Params controlling whether we should trust scanmatching
                    # over initial odometry estimate.
                    "match_filtering":
                    {
                        # Unused.
                        "dist_accept_threshold": 0.06,

                        # Params below apply to difference in pose resulting
                        # from taking the scanmatcher's estimated pose and
                        # subtracting the estimated pose according to odometry.

                        # Mininum value of x translation to accept in meters.
                        # Used to keep from accepting results where the
                        # scanmatcher says that the robot has moved backwards.
                        "x_min_accept": -0.05,

                        # Maximum value of x translation to accept in meters.
                        # Used to filter out outlier scanmatcher estimates.
                        "x_max_accept": 0.12,

                        # Maximum absolute value of y translation in meters.
                        # As our robot doesn't translate sideways, this keeps
                        # Oort from accepting crazy scanmatching results.
                        "y_abs_accept": 0.11,

                        # Maximum absolute value rotation difference in
                        # radians to accept.
                        "theta_abs_accept": 0.07,

                        # Minimum score for scan matching result to be
                        # accepted.
                        "min_accept_score": 0.45
                    }
                },

            # Params when using cartographer's RealTimeCorrelativeScanMatcher.
            # Unused by default.
            "cartographer_matcher":
                {
                    "debug": false,
                    "debug_viewer_scale": 4,

                    "history_length": 30,
                    "resolution": 0.05,
                    "insert_free_space": true,
                    "hit_probability": 0.55,
                    "miss_probability": 0.49,

                    "linear_search_window": 0.1,
                    "angular_search_window": 0.1,
                    "translation_delta_cost_weight": 0.1,
                    "rotation_delta_cost_weight": 0.1,

                    "use_ceres_refinement": true,
                    "ceres_scan_matcher":
                    {
                        "resolution": 0.025,
                        "insert_free_space": false,
                        "hit_probability": 0.55,
                        "miss_probability": 0.49,

                        "occupied_space_cost_functor_weight": 20.0,
                        "previous_pose_translation_delta_cost_functor_weight": 1.0,
                        "initial_pose_estimate_rotation_delta_cost_functor_weight": 100.0,
                        "covariance_scale": 0.50,
                        "ceres_solver_options":
                        {
                            "use_nonmonotonic_steps": true,
                            "max_num_iterations": 50,
                            "num_threads": 1
                        }
                    },

                    "scanmatch_cov_scale": 10,
                    "inflate_scanmatch_cov_scale": 40,
                    "resume_scanmatch_cov_scale": 50,

                    "sm_dist_accept_threshold": 0.05,
                    "sm_dist_accept_x_min": -0.12,
                    "sm_dist_accept_x_max": 0.05,
                    "sm_dist_accept_threshold_y": 0.11,
                    "sm_angle_accept_threshold": 0.10,
                    # Minimum hit percent a scanmatch result has to have for it
                    # to be accepted (else Oort uses the odometry estimate).
                    "min_sm_accept_score": 0.0,

                    "use_second_stage_matcher": false,
                    "second_stage_matcher": {
                        "ceres_scan_matcher":
                        {
                            "resolution": 0.01,
                            "insert_free_space": false,
                            "hit_probability": 0.55,
                            "miss_probability": 0.49,

                            "occupied_space_cost_functor_weight": 20.0,
                            "previous_pose_translation_delta_cost_functor_weight": 20.0,
                            "initial_pose_estimate_rotation_delta_cost_functor_weight": 50.0,
                            "covariance_scale": 6.00,
                            "ceres_solver_options":
                            {
                                "use_nonmonotonic_steps": true,
                                "max_num_iterations": 50,
                                "num_threads": 1
                            }
                        },

                        # Max distance from odometry estimate scanmatching result
                        # can be before Oort falls back to using just the odometry
                        # estimate.
                        "sm_dist_accept_threshold": 0.051,
                        # Max angle from odometry estimate.
                        "sm_angle_accept_threshold": 0.07,
                        "min_sm_accept_score": 0.0
                    }
                }
        },

    # Params controlling loop closure searches.
    "loop_closure_graph_builder":
        {
            "candidate_search":
                {
                    # Number of k nearest neighbor candidates to search. Larger
                    # values increase CPU usage, but also increase number of
                    # loop closure matches.
                    "knearest_neighbors": 5,

                    # When considering a node's pointcloud as a loop closure
                    # candidate, how large of a neighborhood (in number of
                    # nodes on its chain) to use to build the submap for that
                    # node.

                    # And if a node is selected as a candidate,
                    # nodes within max(ceil(submap_neighborhood_size/2), 1)
                    # hops of that node will not be considered as loop closure
                    # candidates. This is as these nodes' scans are accounted
                    # for in the original node's submap.

                    "submap_neighborhood_size": 15,

                    # Given that nodes are given consecutive integer IDs, this
                    # parameter control how close a candidate node's ID can be
                    # before it's considered.
                    "closest_allowed_id_threshold_large": 30,

                    # If the k-nearest candidate search yields zero nodes
                    # using "closest_allowed_id_threshold_large", Oort runs
                    # a less restrictive search using this param.
                    "closest_allowed_id_threshold_small": 5,

                    # Unused.
                    "max_dist_allowed_threshold": 4.0
                },

            # Which scanmatcher to use to look for loop closures.
            # Either "frsm_matcher" or "branch_and_bound_matcher".
            "scanmatcher": "frsm_matcher",

            "frsm_matcher":
                {
                    # Minimum number of neighbors a node can have before it's
                    # rejected from being a candidate.
                    "submap_min_neighborhood_size": 3,

                    # Hit percent threshold for initial rough match.
                    "rough_match_hitpct_threshold": 0.3,

                    # Hit percent threshold for fine scale matching.
                    "fine_match_hitpct_threshold": 0.6,

                    "match_filter":
                        {
                            # If the node ID distance (difference in ID between
                            # candidate node and query node) is less than
                            # closest_allowed_id_threshold_large use thresholds
                            # below to decide whether to accept the loop closure
                            # match result. And for the params below, Oort
                            # defines the match difference as the difference
                            # between the candidate's pose according to the
                            # current estimate, and the pose the loop closure
                            # scanmatch predicts.

                            # If a match's hit percentage is at least
                            # "close_node_hitpct_threshold", and its match
                            # difference is distance less than
                            # "close_node_max_dist", and angle is less than
                            # "match_max_ang_diff", then accept the match.
                            "close_node_hitpct_threshold": 0.8,
                            "close_node_max_dist": 0.5,
                            "match_max_ang_diff": 2.09,

                            # If the node ID distance is equal or greater than
                            # "closest_allowed_id_threshold_large", use the
                            # params bellow to decide whether to accept
                            # candidate matches.

                            # If the hit percentage is greater than
                            # "match_hitpct_threshold", and the match
                            # difference is less than "match_max_dist", then we
                            # accept the match.
                            "match_hitpct_threshold": 0.7,
                            "match_max_dist": 0.5,

                            # If the hit percentage is greater than
                            # "match_hitpct_threshold_high", and the match
                            # difference is less than "match_max_dist_high",
                            # then we also accept the match.
                            "match_hitpct_threshold_high": 0.80,
                            "match_max_dist_high": 2.0

                            # Alternatively, if the candidate's estimated
                            # heading before scanmatching is in the opposite
                            # direction (controlled by
                            # "opposite_dir_threshold"), then use
                            # "match_hitpct_threshold_opposite_dir" and
                            # "match_hitpct_threshold_opposite_dir".
                            "opposite_dir_threshold": 1.74,
                            "match_hitpct_threshold_opposite_dir": 0.67,
                            "match_hitpct_threshold_high_opposite_dir": 0.75,
                        }
                },

            "branch_and_bound_matcher":
                {
                    "debug": false,
                    "debug_draw_original_cloud": false,
                    "debug_only_show_matched": true,

                    "submap_min_neighborhood_size": 3,
                    "global_localization_min_score": 0.21,
                    "min_score": 0.55,

                    "probability_grid":
                    {
                        "resolution": 0.05,
                        "insert_free_space": false,
                        "hit_probability": 0.55,
                        "miss_probability": 0.49
                    },

                    "adaptive_voxel_filter":
                    {
                        "max_length": 0.9,
                        "min_num_points": 150,
                        "max_range": 50.0
                    },

                    "fast_correlative_scan_matcher" :
                    {
                        "linear_search_window": 7.0,
                        "angular_search_window": 0.524,
                        "branch_and_bound_depth": 7
                    },

                    "ceres_scan_matcher":
                    {
                        "occupied_space_cost_functor_weight": 20.0,
                        "previous_pose_translation_delta_cost_functor_weight": 10.0,
                        "initial_pose_estimate_rotation_delta_cost_functor_weight": 1.0,
                        "covariance_scale": 0.125,
                        "ceres_solver_options":
                        {
                            "use_nonmonotonic_steps": true,
                            "max_num_iterations": 10,
                            "num_threads": 1
                        }
                    },

                    "match_filter":
                        {
                            "close_node_hitpct_threshold": 0.0,
                            "close_node_max_dist": 0.5,
                            "match_max_ang_diff": 2.09,

                            "match_hitpct_threshold": 0.0,
                            "match_hitpct_threshold_high": 0.0,

                            "opposite_dir_threshold": 1.74,
                            "match_hitpct_threshold_opposite_dir": 0.0,
                            "match_hitpct_threshold_high_opposite_dir": 0.0,

                            "match_max_dist": 0.5,
                            "match_max_dist_high": 2.0
                        }
                }
        }

    # Params for loop closure matching when Oort gets a hint that the robot
    # has returned to its dock (using the service call map/notify_docked).
    "dock_graph_builder":
        {
            # Number of nodes at the beginning of the graph to use to create a
            # reference submap for matching.
            "dock_submap_size": 31,

            # Number of nodes at the end of the graph to use to create a submap
            # to match to the dock reference submap.
            # IMPORTANT:
            # Should be smaller than dock_submap_size to avoid scenes with lots
            # of aliasing (like hallways).
            "dock_match_submap_size": 7,

            # Size of contacts on the dock. We need this as the robot will
            # report that it's on the dock whenever it touches any part of the
            # contact.
            "dock_contacts_size": 0.15,

            # Max distance we assume user will move dock between when we see
            # it.
            "dock_max_translation": 0.25,

            # Max distance we expect for loop closure constraints to the same
            # dock.
            "dock_match_max_dist": 0.4,

            # Minimum hit percent (overlap) before accepting a dock closure
            # hypothesis.
            "dock_match_min_hit_pct": 0.6
        }
}

@0xfb2c8b83b3af128b;

using Cxx = import "/capnp/c++.capnp";
$Cxx.namespace("oort::capnp");

struct Point2d @0x86eb3abaf30b3dab {
    x @0 :Float64;
    # X coordinate. Can be nan or +inf.

    y @1 :Float64;
    # Y coordinate. If x is nan or +inf, stores the angle in radians of the
    # original sensor beam.

    z @2 :Float64 = 1.0;
    # Stores a homogeneous coordinate 1 for a valid point, or nan which
    # represents either:
    # 1. A point with normal x & y but is less than than min range or greater
    #    or equal to max range in the original laser scan message.
    # 2. A point with a nan or +inf range.
}

struct Pose2d @0xef9347c8a4ec3f3b {
    x @0 :Float64;
    y @1 :Float64;
    t @2: Float64;
}

struct Matrix3d @0x8bf10fa249db2c04 {
    v @0 :List(Float64);
}

struct Pose2dCov @0x8ca5f336b2fcc26d {
    pose @0 :Pose2d;
    cov @1 :Matrix3d;
}

struct Node2d @0xe51b2aac4150d637 {
    id @0 :UInt64;
    pose @1 :Pose2d;
}

struct Sensor2dConstraint @0x9e8d9650484abd02 {
    time @0 :Float64;
    # ID of node this constraint was calculated at (has no implications on the
    # direction of transforms).
    id @1 :UInt64;
    pose @2 :Pose2dCov;
    type @3 :Type;

    enum Type {
        laser @0;
        odom @1;
    }
}

struct Edge2d @0xdcbcc7844d8936a0 {
    id @0 :UInt64;
    i @1 :UInt64;
    j @2 :UInt64;
    # i_T_j
    props @3 :Sensor2dConstraint;
}

struct Edge2dSwitchable @0xaafd6e34d5cc28cf {
    id @0 :UInt64;
    i @1 :UInt64;
    j @2 :UInt64;
    # i_T_j
    props @3 :Sensor2dConstraint;
    switch @4 :Float64;
}

struct Graph2d @0xf0829dca2aeac818 {
    nodes @0 :List(Node2d);
    edges @1 :List(Edge2d);
    edgesSwitch @2 :List(Edge2dSwitchable);
}

struct PointCloud2d @0xf0211b9259d37f5f {
    # ID of node this pointcloud was captured at.
    id @0 :UInt64;
    points @1 :List(Point2d);
    time @2 :Float64;
}

struct Location @0xd16e2058832c0e78 {
    namespace @0 :Text;
    nodeid @1 :UInt64;
    nTsensor @2 :Pose2d;
    uuid @3 :Text;
}

struct IncrGraphResult @0x815530c5e1acb0df {
    # ID of node this result was produced at.
    id @0 :UInt64;
    target @1 :UInt64;
    # posecov: Pose in scan matcher's global frame
    smTid @2 :Pose2dCov;
    # pose_delta: pose between adjacent time steps.
    targetTid @3 :Pose2d;
    hitPct @4 :Float64;
}

struct LoopGraphResult @0xd836671ff6e61ea9 {
    # ID of node this result was produced at.
    id @0 :UInt64;
    target @1 :UInt64;
    idTtarget @2 :Pose2dCov;
    hitPct @3 :Float64;
}

struct SidecarDb @0xad039fab7cfb237c {
    clouds @0 :List(PointCloud2d);

    incrs @1 :List(IncrGraphResult);

    loops @2 :List(LoopGraphResult);
}

struct Map2dMetadata @0xa74d65c2b1e77767 {
    locations @0 :List(Location);
}

struct Map2d @0xd9ca05175c083d2b {
    graph @0 :Graph2d;
    sidecardb @1 :SidecarDb;
}