Update the scatter plot.

Get ray origin and normalized directions in world coordinate for all beams in one scan. :param directions: ray directions in the lidar coord

Render rays by computing the output of @model applied on @rays :param model: NOF model, defined by models.NOF() :param embedding_xy: emb

Calculate the errors.

Get the direction of laser beam in lidar coordinate. :param angle_min: the start angle of one scan (scalar) :param angle_max: the end an

for each vertex in verts2 find the nearest vertex in verts1 Args: nx3 np.array's Returns: ([indices], [distances])

Generate 2D transformation from pose, the rotation counterclockwise about the origin :param pose: shape: (3,) :return:

The model of NOF. :param feature_size: number of hidden units in each layer :param in_channels_xy: number of input channels

Create commands from the Odometer. input: odometer reading output: commands for each frame.

calculate the estimated poses.

Helper function that performs model inference. :param model: NOF model :param embedding_xy: position embedding module :param samples

Initialize particles with a noisy initial pose. Here, we use ground truth pose with noises defaulted as [±5 meters, ±5 meters, ±π/6 rad] to m

Initialize particles uniformly. Args: map_size: size of the map. numParticles: number of particles. Return: parti

Convert particles to the estimated pose accodring to the particles' distribution :param particles: 2-D array, (N, 4) shape :return: a est

Plot the final localization trajectory. Args: results: localization results including particles in every timestamp. poses: ground tru

Render rays from neural occupancy grid, only used for accelerating MCL :param grid: occupancy grid from NOF :param map_size: the size of

Initial drawing of the scatter plot.

Visualize localization results offline. Args: results: localization results including particles in every timestamp. poses: ground tru

Initialization: mapsize: the size of the given map poses: ground truth poses. map_poses: poses used to generate the map.

Embeds x to (x, sin(2^k x), cos(2^k x), ...) :param x: (B, self.in_channels) :return out: (B, self.N_freq * self.in_channels

Encodes input position (xy) to occupancy probability (p_occ) :param x: the embedded vector of a 2D position (shape:

Create commands out of the ground truth with noise. input: ground truth poses and noisy coefficients output: commands for each f

Generate motion reckon only for comparison.

MOTION performs the sampling from the proposal. distribution, here the rotation-translation-rotation motion model input: particles: t

Save the intermediate plots of localization results. Args: frame_idx: index of the current frame. map_size: size of the map. po

Update the scatter plot.