The Foreseeable Future: Self-Supervised Learning to Predict Dynamic Scenes for Indoor Navigation

ICP-based simultaneous localization and mapping (SLAM) algorithms are widely used in robotics, with many variants [37, 56, 30, 11]. We designed our PointMap SLAM with a focus on simplicity and efficiency. Similarly to [11], we keep a point cloud with normals as the map, but we update the normals directly on the lidar frames with spherical coordinate neighborhoods. Our approach targets the problem of robots that navigate in the same environment repeatedly. Therefore we prefer to use PointMap as a mapping tool first and then only rely on the frame-to-map alignment for localization.

Computing occupancy probabilities with ray-casting is also a common technique in the literature. Used at first for 2D occupancy grid mapping [32], it was later adapted for 3D mapping [19, 17]. In our case, PointRay computes occupancy probabilities on a point cloud instead of a grid, similarly to [38], and therefore only models free space where points have been measured. Our main addition is the notion of dynamic and movable labels, which we get by combining multiple sessions. Other minor differences with [38] include a simplification of the probability update rules and the use of frustums instead of cones around lidar rays.

Combining occupancy probabilities and real-time localization is still relatively under-explored. [5] propose to detect short-term and long-term movables similar to our dynamic and movable labels. However, they compute their short-term and long-term features with ray-tracing in a 2D map, while we propose to train a deep network able to predict them directly in the 3D lidar points based on the appearance of the point clouds. Predicting movable points with deep networks was also suggested by [12]. However, they chose a 2D architecture, FastNet, using lidar depth images, and they only predict an objectness score from a human-annotated training set.

Self-supervised learning is a form of unsupervised learning where the data provides supervision. In robotics, this term usually refers to methods using an automated acquisition of training data [46, 26, 16, 6, 39, 33]. These approaches often exploit multiple sensors during the robot’s operation. In our case, we only use a 3D lidar and an algorithm that provides automated annotation for the data. Deep-learning-based semantic SLAM algorithms have been proposed, using either camera images [57, 53], lidar depth images [9], or lidar point clouds [48], but they always rely on human-annotated datasets, whereas our method learns on its own. To the best of our knowledge, our approach is the first to use multi-session SLAM and ray-tracing as annotation tools for the training of semantic segmentation networks.

Following the success of recurrent neural networks (RNNs) and in particular long short-term memory networks (LSTMs) [1, 15], the idea of trajectory prediction has received a lot of attention. It requires the obstacles to be isolated as distinct object and tracked. [21] uses an LSTM-based network to predict obstacle trajectories and plan around them, and [35] exploits a Hidden Markov Model to predict future states from a history of observations. Similarly, [43] detects individual obstacles and predicts their speeds to avoid “freezing zones”. Similar object-centric methods are also used in the context of autonomous driving [28, 8]. However, they all rely on detection and tracking predictions and do not easily incorporate multi-modal predictions, problems we do not face with our point-centric approach. Closer to our work, [20] predicts future human motions as 2D heat maps, implicitly handling multi-modality, but still relies on object-level predictions and is also limited to 2D inputs, where our method leverages 3D features that are more descriptive.

A fair comparison between trajectory prediction methods and our work is nearly impossible because they rely on detection and tracking algorithms, and are usually evaluated with respect to each object in the scene. On the contrary, our occupancy grid predictions are different in nature and are evaluated as a representation of the whole scene. We argue that the difficulty that object-centric methods have to scale with a high number of agents and handle multi-modality, which are two things inherently handled by our approach, justify the relevance of our contributions. In addition labelling object automatically is harder than labelling points automatically.

Reinforcement Learning has been used extensively in recent years to replace standard motion planning algorithms [10, 25, 23, 42, 13, 47, 24]. However, standard local planners have proven to be very reliable methods, especially when it comes to producing dynamically feasible trajectories, which most RL methods fail to do. Even when the feasibility is ensured [34], the whole planning algorithm is embedded into a black box end-to-end neural network, which is difficult to tune, debug, and interpret. We chose to keep a standard local planner, with its guarantees and interpretability, and use a self-supervised deep learning method to predict the future motion of dynamic obstacles.

OGM prediction approaches are the closest to our work and can be separated into two groups either using handcrafted or learned features. Handcrafted approaches usually rely on a model for human motion prediction. [36] predicts a Dynamic Risk Density based on the occupancy density and velocity field of the environment. [18] extends this idea with a Gaussian Process regulated risk map of tracked pedestrians and cars. Other recent works focus on adapting the uncertainty of the predictive model [14, 2, 4], using real-time Bayesian frameworks in closed-loop on real data. These methods either rely on object detection and tracking or are based on instantaneous velocities, and are not able to predict future locations of obstacles accurately. Learned methods, usually based on video frame prediction architectures [27, 54, 55], are better at predicting complex futures. [31] introduces a difference-learning-based recurrent architecture to extract and incorporate motion information for OGM prediction. [44] presents an LSTM-based encoder-decoder framework to predict the future environment represented as Dynamic Occupancy Grid Maps. They introduce recurrent skip connections into the network to reduce prediction blurriness. To account for the spatiotemporal evolution of the occupancy state of both static objects and dynamic objects, [52] proposes a double-prong network architecture.

However, two major differences remain in our approach. First, these methods all take previous OGMs as input, effectively losing valuable information in the shape patterns that common 3D sensors can capture. To our knowledge, we are the first to fill this gap in the literature, by incorporating 3D features in the forecasting of OGMs with the 3D backbone of our network. Second, we are also the first, to our knowledge, to predict a sequence of OGMs without recurrent layers. We argue feedforward architectures are easier to train and understand. Eventually, our network can make a distinction between different semantic classes, leveraging interactions between them, when predicting future occupancy.

PointMap [49] is our SLAM algorithm, which has two components: an Iterative Closest Point (ICP) localization solution that aligns lidar frames on a point cloud map, and a mapping function that updates the map with the aligned frame. Each function can be used independently or together in a SLAM mode.

For a detailed description of the ICP algorithm, we refer to a previous in-depth review [37]. Following their work, we use the same elements to characterize our ICP: the data filters, the matcher, the outlier rejection, the distance function, and the convergence tests. Our choices for each element are listed in Table I. Most of these are based on previous works [30, 11, 49], taking into account that we use a Velodyne HDL-32E sensor. Some elements, including matching, are simplified for efficiency. We use the latest odometry of the robot as the initial pose to solve the initialization issue common to most ICP solutions. In the following, we define transformations by their rotation and translation components $(R,t)$. In comparison to [49], we handle the motion-distortion effect of real data within the iterative process of ICP. At each ICP iteration, after estimating the transformation $(R_1,t_1)$ at time $t_1$ (last timestamp of the current lidar frame), and before matching neighbors, we apply motion distortion. We have the poses $(R_0,t_0)$ and $(R_1,t_1)$ of the lidar at time $t_0$ and time $t_1$, therefore for any point stamped with a time $t\in [t_0,t_1]$, its pose $(R,t)$ is computed as

In addition, we use an optional ground plane heuristic in the optimization. This heuristic, particularly useful for indoor scenarios, assumes that the ground is a planar horizontal surface of height $z=0$. During ICP optimization, any point considered ground can be matched to this plane instead of its nearest neighbor.

The second component of PointMap, the map update function, adds the information from an aligned frame to the map. We use the same update function described in [49], where the map is defined as a sparse voxel grid. The voxel size is ${dl}_{map}=3$cm and we keep only one point per voxel, with its normal and its score. When using an initial map for localization, we can create a secondary map that we ignore for localization, but keep for further processing, which is particularly useful for the buffer creation step shown in Figure 4 (a).

We use PointRay [49] to compute occupancy probability in a point cloud map. The occupancy probability of each point in a map can be found thanks to the data provided by lidar frames. Indeed, each frame provides two kinds of information: occupied space where the points are located, and free space along the lidar rays. If a location exists in the map but gets passed through by multiple rays, it will have a low occupancy probability. We follow the idea from [38] and use the projection of the map in the frame spherical coordinates to model the lidar rays. The occupancy probabilities are deduced from the distance gap between frame points and map points.

PointRay assigns two values to each point of the map $x_i$, in a voxel $i$: $n_i$ the number of times this voxel has been seen, and $o_i$ the number of times this voxel was occupied. For each lidar frame in the list, PointRay first gets the list of occupied voxels, and increments both $n_i$ and $o_i$ for them. For the rest of the points, PointRay verifies if they are seen by a free space frustum in the spherical coordinates $(\rho ,\theta ,\varphi )$. The frustums are defined as the pixels of a 2D grid in the $\theta$ and $\varphi$ spherical dimensions. Each pixel (or frustum) stores the smallest point distances to the lidar origin ($\rho$ spherical coordinate). The resolution of the grid is $d\theta ={0.33}^{\circ }$ and $d\varphi ={0.5}^{\circ }$, but in the $\theta$ dimension, the lidar resolution is variable, so we use nearest-neighbor interpolation to fill the empty pixels along this dimension.

The verification is done by projecting the map in the same frustum grid. To reduce the effect of motion distortion, we cut the lidar frame in $n_{slices}=16$ slices along the azimuth and perform the map projection with the median pose of each slice. Given the small slicing angle, the distortion effect is negligible. Although it is slower to compute the occupancy probability one slice at a time, we alleviate the computational cost by only projecting map points that are in the slice area. For every projected map point $x_i$, PointRay increments $n_i$ (and not $o_i$) only if the two following conditions are respected:

When all the lidar frames of a session have been processed, the final occupancy probability for each point $x_i$ of the map is computed as $p_i=o_i/n_i$. A point has to be seen at least $n_{min}=10$ times to be considered valid, otherwise, $p_i=0.5$.

In our annotation pipeline, one of the issue we face when dealing with real data is the noise. As shown in the middle-bottom picture of Figure 4, the point labels found automatically can be noisy. In this section, we define Mathematical Morphology operators for point clouds to help reduce the noise in the annotations, through spatial smoothing.

Mathematical Morphology regroups techniques for processing geometrical structures and was originally developed for binary images, with four basic morphological operators: erosion, dilation, opening, and closing [29]. Some works have tried to adapt mathematical morphology to point clouds, by considering points as positive elements, and empty space as negative elements [7, 3].

In this work, we are interested in a simpler problem: applying mathematical morphology on point clouds where some points are considered positives and the other negatives (see Figure 3). We only consider a sub-problem where the structural element is a sphere of radius $r$, the positive elements are denoted as point cloud $A$ and the negative elements as point cloud $B$. Therefore, the morphology operations can be described simply:

• Dilation: $D_r(A,B)=A\cup N_{B,r}\left(A\right)$

• Erosion: $E_r(A,B)=A\setminus N_{B,r}\left(A\right)$

• Closing: $C_r(A,B)=E_r(D_r(A,B),E_r(B,A))$

• Opening: $O_r(A,B)=D_r(E_r(A,B),D_r(B,A))$

Where $N_{B,r}\left(A\right)=\{p\in A|\exists q\in B:\parallel p-q\parallel \le r\}$ is the subset of $A$ in the neighborhood of $B$. As we will see in Section IV-B, these tools are particularly efficient for removing noise and undesired artifacts in the annotations.

In this work, we assume that our robot will be navigating in the same space for some time, and thus will use a map of this environment for localization. This map will also be used during the annotation process. As explained in our previous works, for the annotation, we need the map to contain only ground and permanent points. It is also convenient for localization, as these points are usually from large planar surfaces, easy to localize against.

The mapping process starts by using PointMap in SLAM mode to get a first map of the environment. We then perform a loop closure using the Open3D library [58]. The map created from the loop-closed poses is then cleaned from its dynamic objects with PointRay. As the spaces for our real experiments have windows, we hard-code map limits, to remove the outdoor space that is mixed with reflections of indoor space. To ensure the map is also cleaned from any movable object, we perform a few other runs where the movable objects in the space have been moved to different places. These refinement runs are given to PointRay to compute occupancy probabilities. Points with a low occupancy probability are removed, and we eventually obtain a good quality map, containing only ground and permanent points. As it is usually the case in indoor environments, the ground is flat, allowing us to use the flat ground heuristic from PointMap. As a consequence, the ground point can easily be extracted as the horizontal plane at $z=0$.

A human could intervene at this step to move furniture around between mapping sessions, but it is not required. We assume furniture would be moved at some point in the robot life, and only do it ourselves for convenience in our experiments. We also note an interesting idea: this strategy could be used for cheap and fast 3D point cloud annotation of any object. For example, only remove the chairs first to annotate them, then remove tables, etc.

We use an automated annotation process to label the 3D lidar points [49] and generate training SOGMs [50], allowing self-supervised learning. Our network can thus learn from new situations encountered throughout the robot’s life.

We first annotate lidar frames, with the combination of PointMap and PointRay, as shown in Figure 4. As opposed to [49, 50], we are able to handle noisy and imperfect real data. We consider that our map has already been refined and we focus on the automated annotation of the lidar point cloud labels. The lidar frames of the session are aligned on the map with PointMap, and a buffer of new points is created. To annotate the permanent points, we perform a point-morphology closing of the map permanent points on the buffer points ($r=0.9$m). To annotate the ground point we perform another closing of the map ground points on the buffer points ($r=0.2$m). This leaves us with the remaining buffer points that are processed by PointRay, to get their occupancy probabilities, $p_i$, during this session. Points with $p_i<{\tau }_s=0.3$ are labeled as dynamic, points with $p_i>{\tau }_m=0.6$ are labeled as movable, and the remaining points are left uncertain. We conduct a noise removal step consisting of a first closing of dynamics on movables ($r=0.12$m) to clean the isolated or tiny groups of movable points inside dynamic areas. And then a second larger closing of movables on dynamics ($r=0.9$m) to ensure we only keep large groups of dynamic points. Eventually, the annotations are projected back on the lidar frames, taking into account motion distortion.

Now that we have annotated lidar frames, we can generate SOGMs automatically. Our SOGMs have three channels, one for each obstacle class: permanent, movable, and dynamic. Because we want to be able to augment the training data with rotations, it is better to save our 2D labels as intermediate 2D point cloud structures that can easily be rotated and then transformed into SOGMs during training. An annotated 2D point cloud is computed and saved for each lidar frame by removing non-obstacle points, projecting the remaining points on a horizontal plane, and subsampling them with a grid size of $3$cm. To reduce the noise, we remove isolated dynamic points from these precomputed 2D point clouds, and perform a point morphology opening of the dynamic points by the static points (permanent + movable), with $r=0.3$m.

At training time, we stack the 2D points in a third dimension according to their timestamps, apply data augmentation, and project them to a SOGM structure of spatial resolution $d{l}_{2D}=12$cm and temporal resolution $dt=0.1$s. The permanent and movable occupancies from all time steps of the SOGM are merged because they are not moving. Therefore, in addition to the future locations of dynamic obstacles, our network also learns to complete partially seen static objects.

Our network architecture (Figure 6) is composed of two parts, a 3D back-end, and a 2D front-end. The 3D back-end is a KPConv network [51] predicting a semantic label for each input point. Predicting 3D labels helps the network training by providing an additional supervisory signal and ensures that rich features are passed to the 2D front-end. We keep the KP-FCNN architecture and parameters of the original paper: a U-Net with five levels, each composed of two ResNet layers, refer to [51] for details. The network input is a point cloud made from $n_f=3$ lidar frames aligned in the map coordinates and merged. We only keep the points inside a $R_{in}=8$m radius, as we are interested in the local vicinity of the robot. Each point is assigned a one-hot $n_f$-dimensional feature vector, encoding the lidar frame to which it belongs. To help with computational speed for inference on a real robot, the input point density is controlled using a relatively large grid subsampling size ($d{l}_{3D}=12$cm). It is equal to the PointMap subsampling size, allowing us to reuse the subsampled cloud aligned and rectified by PointMap as is.

The 3D point features of dimension are passed to the 2D front-end with a grid projection using the same spatial resolution $d{l}_{2D}$ as the SOGM. The size of the grid is determined as the inscribed square in the $R_{in}$-radius circle: $h_{grid}=w_{grid}=94$. Features from points located in the same cell are averaged to get the new cell features. The features of the empty cells are set to zero. The obtained 2D feature map of dim is then processed by an image U-Net architecture with three levels, each composed of two ResNet layers, to diffuse the information contained in sparse locations to the whole grid. This dense feature map is used to predict the initial time step of the SOGM. Then, it is processed by successive propagation blocks, each composed of two ResNet layers. The output of each propagation block is used to predict the corresponding time step of the SOGM. We define the final prediction time $T=4.0$s, meaning that our SOGMs have $n_T=T/dt+1=41$ time steps in total. More details and hyperparameters can be found in our implementation. Note that the permanent and movable predictions are redundant but we keep them to help the network to keep the knowledge of their location, and to learn better interactions between the classes further into the future.

The training loss of our network is a combination of two loss functions. The standard semantic segmentation loss of a KPConv network $L^{3D}$, and a loss function applied to each SOGM prediction layer $L_k^{2D}$. We define it as

Our network is trained with PyTorch, with an SGD optimizer. The initial learning rate is $0.01$, and decayed by 0.96 at every epoch (equivalent to 0.1 every 60 epochs). In this setup, our input point clouds contain on average $20$k points. We use a variable batch size targeting $B=6$, for an average of $85$k points per batch. During training, we only use rotation augmentation around the vertical axis. To cope with unbalanced classes in real data, we implement a sampling strategy targeting input examples containing dynamic points. Instead of sampling frames randomly during training, we chose the frames that contain dynamic points more often than the rest of the frames (with a ratio of 10:1). We also have the possibility to train a network with a combination of real and simulation examples (with a customizable ratio), a strategy that we evaluate in section VII-C. The rest of the training parameters are kept identical as in the original KPConv paper [51], and more details can be found in our open-source implementation.

During navigation, our network receives lidar point clouds sent by PointMap. They are already subsampled to $d{l}_{3D}=12$cm, aligned on the map, and motion rectified. When three consecutive point clouds have been received, the CPU processing kicks in and performs all the necessary operations including merging frames, subsampling layer points, computing neighbors, etc. As soon as the CPU pre-processing is over, the GPU computes a forward pass of the network and gets the predicted SOGM, which contains the future occupancy locations up to 4 seconds after the input lidar timestamp.

We want to use these predictions in the Timed-Elastic-Band (TEB) planner [40, 41], but the original implementation only handles point obstacles. We chose to modify the TEB implementation to be able to handle grid representations (see Figure 9). TEB originally minimizes a linearly decreasing cost function: $C_{obst}=max(0,1-d/d_0)$, where $d$ is the distance from the optimized pose to the closest obstacle and $d_0$ a predefined influence distance. The simple way to handle grid structures is to let TEB minimize the risk value at the current pose (interpolated from the closest grid values). Then we just need the grid values to represent a smooth cost function. In our case, we defined a linearly decreasing risk value similar to the original obstacle cost but with some modifications. First, we use a threshold ${\tau }_{risk}=0.4$ to extract risky area from the SOGM:

Summing risk this way means larger risk areas will have higher risk values. To even out the risk value for any risk area, we apply the same convolution but on normalized risk:

In addition to this new definition of SRM, we decouple the static risk and dynamic risk. The dynamic risk is computed from the dynamic channel of the SOGM, while the static risk comes from the permanent and movable SOGM channels. Because the static risk is the same at any time, it can be stored in a single 2D risk map. For convenience, we store it in the first layer of the SRM, while the rest of the SRM layers only contain the dynamic risk. This decoupling allows us to have separate distance and weight parameters for both risks, and keep better control of the navigation behavior. The dynamic risk is diffused in space and time defined as above with $d_0^{dyn}=1.2$m, and ${\Delta }_0=1$s and the static risk is diffused only in space with $d_0^{sta}=0.9$m.

If a pose time is too far in the future, it ignores the dynamic risk. TEB also allows the optimization of multiple trajectories for different homotopy classes. We keep this feature by creating estimated point obstacles at local maxima in the SOGM, ignored by the trajectory optimizer, but used for homotopy class computation.

In simulation, we have access to high computing power, with an Nvidia RTX 3090 GPU and an Intel i9-10980XE CPU @ 3.00GHz. In addition, we can slow the simulation time factor to reduce delay to virtually zero if we need. On the real robot, we are limited to a laptop configuration, with a much smaller GPU (Nvidia T1000) and a much slower CPU (i7-11800H @ 2.30GHz). However, with our current implementation and parameters (especially $d{l}_{3D}=12$cm), we can run everything in real-time. The input frames arrive from PointMap with a delay of approximately $45$ms. The CPU pre-processing takes on average $47$ms and the GPU network computations $74$ms. Finally, $53$s are used for the SRM conversion. The total delay to get a new prediction varies between $230$ms (${10}^{th}$ percentile) and $320$ms (${90}^{th}$ percentile), with an average of $259$ms, which is totally acceptable. It means only the first few layers of the predictions are obsolete. To remain closer to the real configuration in our simulation experiments, we simulate this delay by waiting before publishing the network results to the rest of the pipeline.

Our network predictions can easily be plugged into a standard navigation system. We use original ROS plugins for most of the navigation except for localization, which is performed with PointMap (adapted as a ROS plugin), and the local planner, which is our modified version of TEB. As shown in Figure 10, in the standard navigation pipeline, lidar frames are processed by PointMap to get the current robot pose. Then local and global costmaps are computed with the move_base ROS node. The global planner finds the optimal path to the goal and TEB follows this path while avoiding obstacles in the local costmap.

When using deep predictions, the subsampled and aligned frame from PointMap is sent to the network to be labeled, and to produce the SOGM, immediately converted into SRMs. The global costmap is computed with the labeled points and ignores dynamic obstacles. TEB tries to follow the global plan while avoiding high-risk areas in the SRMs. Note that we use the raw lidar frame for localization as opposed to [49], where we used predicted frames, because our network needs three aligned input frames to be able to extract the current speed of dynamic points.

We use the same Gazebo simulated environment as in [49, 50] for our experiments. In this case, we designed a controlled experiment that could be repeated many times to get reliable results and fair comparisons between all methods. Tables and chairs are generated randomly in the space and a fixed number of Flow Followers [50] are moving between a set of goals that we chose around the robot path, to force a lot of encounters. The robot is always asked to follow the same path, consisting of going across the main atrium a few times. See Figure 11 for a visualization of this setup.

In our experiments, we use metrics that are simple and intuitive. To measure the efficiency of the planner, we use the Time to Reach the Final Goal ($T_f$) $\downarrow$ in seconds. To measure the safety of the planner, we measure the distance from the center of the robot to the center of the closest dynamic actor (whose position is given by the simulator), and derive two metrics from it: the Collision Ratio (%C) $\downarrow$, measuring the percentage of the total time during which the robot is in collision with an actor (distance smaller than $d_c=0.4m$); and the Risk Ratio (%R) $\downarrow$, measuring the proportion of the session during which the robot is in a risky area (distance smaller than $d_r=1.0m$), which indicates when the robot is dangerously close to an actor. In addition to these main metrics, we provide four additional metrics providing more insight into the results:

• average absolute speed (AAS) $\uparrow$

• percentage of time stopped (%S) $\downarrow$

• average linear speed (ALS) $\uparrow$

• percentage of time going backwards (%B) $\downarrow$

In our first experiment, we verify the benefit of our Deep-SOGM predictions for navigation. We thus keep the TEB planner and its parameters fixed and compare its performances when using:

• NoPreds: original version of the TEB ROS package, using local costmap pixels as obstacles.

• IgnoreDyn: idea from [49], ignoring the dynamic obstacles, only using the static SOGM.

• LinSOGM: use actor current speeds (provided by the simulator), and extrapolate their positions linearly.

• DeepSOGM: our deep SOGM predictions. We use the network trained on Flow Followers from [50], and use it for inference here.

• GtSOGM: precompute actors’ movements in advance, to get the groundtruth future SOGM when navigating.

First, we look at TEB without predictions, NoPreds, and see that it nearly never collides (%C $<0.5$). However, it often gets into risky situations and has to stop, therefore, it ends up being inefficient, with a longer time to finish. The first idea, IgnoreDyn, proposed in [49], is to ignore dynamic obstacles, hoping for the fact that they will avoid the robot on their own. Even if the Flow Followers are implemented to avoid the robot, they end up colliding often with it ($2$%C on average). We argue that people would be better at avoiding the robot, but it would be risky to rely solely on people’s reactions, and it would probably lead to many collisions. We notice that this planner is the fastest ($T_f$ < $140$s on average), and rarely stops, because it goes straight to the goal, without avoiding the dynamic obstacles. Then we evaluate a planner using the linear extrapolation of the actor’s current speeds, LinSOGM. This method gives the robot a sense of the future movement of the actors, which leads to a reduced time in risky and collision areas. But it is not ideal yet, because the robot anticipates on an approximate linear prediction, and has to readjust many times. It particularly affects the efficiency (time to finish) and is even worse than the regular TEB. Our version of TEB, DeepSOGM, performs well compared to the other methods, with close to zero collision (%C $<0.5$), the shortest time in risk areas ($5$%R on average), and a relatively fast finishing time. Eventually, we compare the performances to the SOGMs using the actual groundtruth future provided by the simulator, and this is probably the most impressive result. Our performances are extremely close to the performances of a robot that could actually predict the future. We believe this result is more useful than a comparison to other OGM prediction methods, which would be complex to implement in our pipeline without modifying several other components. It lets us believe that our prediction method, in itself, is strong enough to provide SOGMs of sufficient quality for navigation and that further improvements would probably be achieved by upgrading other components (SRM conversion, planner, etc.), or finding ways to prolong the prediction time horizon.

In our supplementary video, we show the robot navigating with different types of prediction, first in rviz view, where predictions can be visualized, and then in a schematic bird-eye view where the difference in trajectories between NoPreds and DeepSOGM can be seen.

In this second experiment, we use the same protocol to compare three versions of our Planner using our Deep-SOGM predictions. We show how some of the key choices made for the SOGM-to-SRM conversion affect the performances in terms of safety and efficiency in Figure 14. First, we measure the performances when computing SRM with $p=1$. The navigation is riskier and less efficient because the risk function is not well defined in the space between obstacles. Then we remove the diffusion of the risk in the time dimension, one of the additions made in this work. In that case, the robot can plan trajectories closer to the back or the front of moving actors, which naturally increases the time spent in risky areas. It allows the robot to go faster in some cases, but also means it will end up more often in collision situations, where it has to stop and reverse. Therefore, the distribution of time to finish is more spread out for this method. In both cases, our final version of the planner has better performance.

For the real-world experiments, we use a Clearpath Jackal robot, shown in Figure 15. It is a small field robotics research platform, with an onboard computer, GPS, and IMU fully integrated with ROS. In this work, we use a single 3D sensor: a Velodyne VLP-32C lidar sensor. An RGBD camera is mounted on the robot, but we do not use it for our experiments. To this platform, we add a laptop computer with an Intel CPU (i7-11800H @ 2.30GHz) and an Nvidia GPU (Nvidia T1000 4GB). Most of the computations (localization, planning, inference) are performed on the laptop. Only basic tasks (Velodyne processing, low-level control) are performed on the onboard Jackal computer.

In the real world, it is hard to reproduce multiple experiments as we could do in simulation. On the one hand, if you choose to navigate “in-the-wild” in a space where people are not told to behave in a particular way around the robot, the navigation conditions from one session to another will be totally different depending on how people react to the robot or try to confuse it. On the other hand, if you choose to have a controlled experiment, where people are told to act in a particular way, you can assume that the navigation conditions will be roughly similar, but you limit yourself to over-simplified situations and behaviors, and cannot be sure that the results will generalize well to any circumstances.

Having verified our navigation system performances in simulation, another thorough study on our real-world platform is not crucial, and we decided to conduct both controlled and in-the-wild experiments to validate that our robots behave as intended and to confirm the results from Section VI. This is why we perform experiments in two different spaces of the same building. The Atrium has several tables and chairs that are often moved and configured differently depending on the occasion. In this space, students usually come to work and thus dynamic obstacles are not very common, except during specific events. This space was used to conduct more controlled experiments. The main Hall of the building has a big entrance, stairs, and elevator that lead to classrooms, and large open spaces without tables, where students come and go depending on where they are heading. During peak hours, this space can be crowded with dynamic obstacles, which was ideal for our in-the-wild experiments. Pictures of both spaces are shown in Figure 15.

Our first real experiments were conducted in the Atrium because it is ideal for controlled scenarios. With low traffic, and people mostly sitting for long periods, it is less likely that unexpected circumstances arise there. Following our automated annotation pipeline, we first conducted a mapping session, obtaining the initial map shown in Figure 4. Mapping and refinement were done by driving the robot manually for convenience, but could also have been done with the NoPreds-Nav, with PointMap in SLAM mode. We started by collecting a first batch of 9 sessions in the space without interfering. Therefore, only a handful of dynamic obstacles were encountered. A second batch of 10 sessions was collected in a controlled manner, where a person was asked to cross the robot’s path perpendicularly at three different predefined points (shown in Figure 16). The third batch of 6 sessions was collected with a focus on face-to-face encounters. The robot was asked to navigate along a looping trajectory and a person was told to walk along the same loop in the opposite direction (See Figure 16). Finally, we collected 15 additional sessions during a conference that was organized in the building. For these sessions, the layout of the Atrium was different and more people were present in the space without any instructions on how to behave around the robot.

For more “in-the-wild” results, we also collected data in the main hall of the building. After the initial mapping and the refinement runs, we collected 38 sessions, at different hours on different days, alternating between crowded times (See Figure 16) and more calm moments. The sessions collected in this space are not organized in a particular order, because they all were collected without any instructions given to the people in the space. In these sessions (and also in the last batch of sessions in the atrium), we noticed several people trying to mess with the robot by acting in unexpected ways and sometimes had to stop the robot ourselves to avoid collisions. Ideally, we would like the robot to be able to predict any kind of behavior, even the disrupting ones, which is why we keep these sessions in the dataset. Most of the sessions were collected using the NoPreds-Nav, and some later sessions were collected with the DeepSOGM-Nav, using a network trained on earlier sessions. From a data collection and annotation point of view, this does not really matter. A different robot’s behavior may induce different reactions from the people around it, but these cases rarely happen. The collected sessions are listed with details in Tables II and III. Our open dataset is called UofT-Indoor-3D (UTIn3D) and is available for the community to use. We share the lidar frames, the trajectories computed by PointMap, and our annotations. 3D lidar datasets with crowds of indoor pedestrians are not very common, and we hope that UTIn3D will be beneficial for the community.

We do not have quantitative measurements of the quality of the annotation on real data, but we can observe the results qualitatively. For the most part, the annotation quality is very good. The different classes are quite well split, with only a few leaks from one class to another, for example where people are close to tables and then moving away, as we can see in Figure 16. This type of mistake does not affect our navigation system so much as most encounters are happening far away from static objects like tables. Examples of annotated SOGMs can be seen in Figure 17.

In this section, we focus on the evaluation of the network SOGM predictions. Similarly to [50], we compare the predicted SOGMs to labeled SOGMs annotated by our automated pipeline, using the same metrics, and considering only the dynamic class. In our first experiments shown in Table IV, we compare the performances of our network when trained on more and more data. The results are measured with mean Average Precision computed on the layer of SOGMs at 1, 2, and 3 seconds into the future. We also measure the total Average precision on the whole SOGM. The relatively low values in this table are explained by the complexity of the task. The future movements in the scene are never written in advance and can be multimodal (several possible trajectories are always possible). Therefore, the predictions incorporate this uncertainty and become blurry as time advances. It means lower precision scores, which reduces the values for our metric. The actual performances of our predictions can be judged with the qualitative visualizations we provide.

First, in a lifelong learning manner, we use the UTIn3D-Atrium dataset, which has been split into 4 parts, and we train networks on increasing amounts of the dataset. We test on both UTIn3D validation sets, but we value the validation more in UTIn3D-Hall, as it contains a lot of "in-the-wild" dynamic obstacles. For this experiment, we see that increasing the amount and the diversity of data helps the network achieve better performances, which validates our assumption that a robot using our algorithm would be able to improve itself throughout its life. Because for other experiments, we used simulated data, we have a good opportunity to test the ability of our network to generalize to combinations of real-world and simulated data. We first notice that using only simulated data, the predictions are useless. Even if our simulated space is a copy of the UTIn3D-Atrium space, Sim-to-Real transfer is a very complex problem that we do not expect to solve here. However, when combining simulation data with real data in the training set, we see that the results are improved. The more we add simulated data to UTIn3D-Atrium, the better the performances are. It means, that without seeing any real data, the network is not able to generalize to this new unseen modality, but when given a few examples of the real data, the network can leverage the diversity of dynamic obstacles in the simulated data, at the same time as the specificity of the real data, to improve its performances. We believe that this is due to the fact that UTIn3D-Atrium does not contain a lot of dynamic obstacles (as shown in Table II). The network only needs some frames of the dataset to adapt to the particularity of real data and get better results when it relies more on the movements seen in the simulated data, with many more actors.

Then we also add UTIn3D-Hall to the training data. We notice a big step up in the performances on both validation sets. Following our analysis of the combined results with simulation, it makes sense, because UTIn3D-Hall contains a lot of dynamic obstacles (as shown in Table III), with diverse behaviors. We notice that by combining both real training datasets, we achieve much better results than by combining one real dataset and simulated data. We could expect this, because, like the simulated data, UTIn3D-Hall contains a lot of dynamic obstacles, but it is not very different in nature from UTIn3D-Atrium. It is interesting to note that the gap between simulation and reality affects the performance more than the gap between two different spaces with different room configurations. This result confirms that our network does not overfit the training data and that its predicted SOGMs can generalize well to multiple different spaces.

Eventually, we combine everything, achieving the best results of all on both validation sets. This final result finally confirms that our network has the ability to generalize to combinations of diverse real-world and simulated spaces. The more data we provide, the better the results become, which is exactly the goal of our approach, as the robot should be able to collect this data on its own throughout its life. Interestingly, when we add more simulated data to the training set, we see the opposite as before: a reduction in the performance. In this case, when the real dataset is large enough, we think that adding some examples of simulated data helps by providing more diversity, but relying too much on it makes the network worse on real validation.

We complete these quantitative results with qualitative examples of SOGM prediction for different training sets in Figure 17. Similarly to [50], we use a merged representation of the SOGM, where dynamic predictions from all layers are colored in red, with the corresponding labeled SOGM superimposed as a green contour. The better performances of the network trained on A+H+S50% can be visualized in all three examples. In example (a), we see two people walking in relatively free space. Using only A or A+S50% data, the network is not able to predict the trajectories very well, as it has never seen the Hall. When using A+H, the trajectory gets better and takes the shape of a banana distribution, a phenomenon we previously saw in simulation [50]. Finally, adding simulation with A+H+S50%, helps refine the predictions, with a banana shape much closer to the green contour. In example (b), we notice that without simulation (A and A+H), the predictions tend to merge for groups like these two persons. With simulation data (A+S50% and A+H+S50%), which contains a lot of examples with multiple actors, the predictions for groups get better. Eventually, in example (c) we notice groups of standing people that are classified as movable objects by the first networks and eventually classified as dynamic with A+H+S50%. We do not consider this as an improvement, but more as an open question: should standing people be classified as movable or dynamic? Here this is decided by the network itself, depending on the data it has seen. In UTIn3D-Atrium data, people are standing for long periods, as they are discussing in the context of a conference, in UTIn3D-Hall, with a lot of passage, people are usually stopping only briefly, but then moving again, which explains the difference in the predictions.

In addition, more examples of predictions from A+H+S50% are shown in 18 to visualize the capabilities of our best network on real data. We show simple examples with only a few people (1-5), multiple people walking together as groups (6-10), and complex crowded scenes (11-15). Among these examples, we can find many interesting predictions. First, we notice the banana shape distribution (better seen as an animated SOGMs in the video) for one person but also groups (2, 4, 6-10, 14). We also see predictions of people fading when they get into elevators or stairs (4, 5, 13), or predictions expecting people to avoid the robot (5, 7, 9, 10).

The final step in our work is to test our navigation system in the real world, to validate the results we had in Section VI in simulation. In this section, we collect qualitative results and anecdotal examples, because we cannot reproduce a fair and accurate experimental process matching the one we have in simulation. We do not have groundtruth information that the simulator would provide, and more importantly, it is very hard to reproduce multiple experiments with the same conditions to compare different methods.

First, we conduct a controlled experiment where people are asked to cross the path of the robot perpendicularly and compare the reactions of the robot when using the standard NoPreds-Nav or when using our DeepSOGM-Nav with and without time diffusion. For this experiment, we repeated the session two times for each system and collected the data. After annotating it, we could collect the distance to the closest dynamic obstacle and compute similar metrics to the ones used in the simulated experiments in Section VI. We use different thresholds for the risk ratios, adapted to the real setup we defined: a Low-Risk Ratio measuring the proportion of the session during which the distance is smaller than $1.5m$), and a High-Risk Ratio measuring the proportion of the session during which the distance is smaller than $0.6m$). We also add two more metrics based on encounter statistics. For this, we segment out each encounter between the robot and a dynamic obstacle as the period when the distance is smaller than $1.5m$. For each encounter we measure the duration and the minimum distance, then we average these values per session.

In Table V, we find that DeepSOGM-Nav has the best performance in terms of safety and efficiency. It is very good at avoiding high-risk areas and keeping a higher minimum distance when crossing the path of people. We notice that without time diffusion for the SRM, DeepSOGM-Nav is faster but a lot riskier. Overall, even though we cannot ensure the exact same conditions every time, get enough repetitions for a good statistical evaluation, and get groundtruth distance measurements; we still obtain similar results as in the simulation experiment.

We also show what happens qualitatively during these experiments in Figure 19. We see that without predictions, the planner sees an object coming from its left, and plans to avoid it by turning to the right. The closer this object gets, the further to the right the planned trajectory is “pushed”, until the point where the robot has to stop and readjust its trajectory to pass on the left behind the person. By doing this the robot gets into a risky situation. On the contrary, with predictions, the planner anticipates that the person is going to be on its right in a few seconds, and, from the beginning, plans a trajectory that passes behind the person, which is much safer and more efficient. The reactions of the robot when using predictions are much more similar to what normal persons would do when crossing each others’ paths.