Map-based simultaneous localization and mapping

Abstract

There is provided techniques for map-based SLAM. The method comprises obtaining new exteroceptive measurements from a device. The new exteroceptive measurements are to be added to a global map. The exteroceptive measurements yield new nodes and new edges in the global map. The method comprises identifying at least one loop closure for the new nodes and the new edges in the global map. The method comprises combining the identified at least one loop closure with previously detected loop closures in the global map, yielding a condensed global map. The method comprises obtaining edge weights for the edges of the at least one loop closure in the condensed global map. The method comprises updating the global map with new inliers and outliers as identified using the edge weights. The edges classified as inliers are added to the global map. The edges classified as outliers are removed from the global map.

Description

[0001] MAP-BASED SIMULTANEOUS LOCALIZATION AND MAPPING

[0002] TECHNICAL FIELD

[0003] Embodiments presented herein relate to a method, an image processing device, a computer program, and a computer program product for map-based Simultaneous Localization and Mapping.

[0004] BACKGROUND

[0005] In scenarios where mobile devices, such as mobile robots or extended reality (XR) devices are involved, the physical environment surrounding the mobile devices as well as the accurate location of the mobile devices in the physical environment needs to be known. This can be achieved by Simultaneous Localization and Mapping (SLAM), where, accordingly a map of the physical environment, and the trajectory of the mobile device in that map, can be estimated using information as obtained from the mobile device’s sensors.

[0006] If instead of a single mobile device, there are multiple such mobile devices exploring a physical environment, both the accuracy and exploration time can be improved by means of collaboration. Hence, to increase the accuracy and reduce the exploration time of the estimation and to be able to localize every mobile device with respect to the same map, Collaborative SLAM (C-SLAM) techniques have been developed, where a single global map is estimated as well as one individual trajectory of each mobile device in that map.

[0007] One approach for SLAM and C-SLAM is graph-based SLAM, where the map itself is abstracted as a graph. This graph consists of nodes, representing the poses of the devices, and edges, which relate the poses of one or several devices to each other. Without loss of generality, only pose graphs will be considered in detail hereinafter. However, the teachings of the present disclosure apply also to more general graphs that, for example, contain landmark / feature nodes. A loop closure edge is generated if a given device revisits a location the given device has seen before (referred to as an intra-agent loop closure) or the given device visits a location another device has visited before (referred to as an inter-agent loop closure). Hence, intra-agent loop closure edges connect two non-consecutive nodes from the same device, whilst interagent loop closure edges connect nodes from different devices which each other. Loop closure edges in the graph can be utilized to reduce the location uncertainty. This can be achieved by aligning the different devices in a common global reference frame. The graph itself is obtained from a so-called front-end which processes the measurements of the devices to obtain the edges between the nodes. The front-end in C-SLAM could be distributed among the devices but it could also be centralized in a server. Further, a so-called back-end is responsible for optimizing the graph (as generated by the front-end) to obtain estimates for the poses of the devices, which, again, could either be distributed or be centralized. This process is referred to as pose graph optimization (PGO) and is often formulated as a non-linear least squares problem as follows:

[0008] Nm

[0009] X* = arg nun ^ r(yi;A)2, i=0 where X represents all poses of all devices, is the ith measurement that corresponds to an edge in the global graph, r(-,-) is a non-negative function that indicates a residual value, and Nmis the number of measurements. This problem can be solved in a centralized manner or in a distributed manner.

[0010] However, often the measurements contain outliers. Here, each outlier represents a pose that is incorrectly linked to another pose. In turn, this results in incorrect relations between nodes in the graph. These outliers occur most often in the loopclosure edges. A common approach to make the PGO more robust is by using an M- estimator approach, which yields the following pose graph optimization problem:

[0011] X*, w* = arg e [0,1] are weights that will be small if a measurement is considered an outlier and ‘T’p (•) is a function that depends on the M-estimator p(-) chosen.

[0012] The pose graph optimization problem can be solved in an iterative manner, where optimization is to be performed alternatively over the poses X and the set of weights w. Pose graphs optimizers that utilize an M-estimator approach are, for example, Robust map optimization using dynamic covariance scaling (hereinafter referred to as DCS) and Graduated Non-Convexity for Robust Spatial Perception: From Non- Minimal Solvers to Global Outlier Rejection (hereinafter referred to as GNC).

[0013] While intra-agent loop closures often can be well-handled due to the odometry backbone of each device, performing global map estimation in C-SLAM can be difficult when the initial relative position and orientation, represented by so called priors, of the devices in a global reference frame, are unknown. If priors are available, they can improve the accuracy of robust pose graph optimization. While, for example, satellite positioning systems can be used to obtain a prior for a device in an open outdoor environment, obtaining priors in a global reference frame is difficult when devices are in an indoor environment or in an outdoor environment, where signals from satellite positioning systems are unreliable, such as the urban canyon effect. Hence, the relative positions between devices must be estimated only from relative measurements between devices and, in this case, outliers can have a large detrimental impact on the global map estimate.

[0014] If the number of outliers becomes too large these methods perform either badly or become too slow to be applicable in real-time applications. Therefore, the measurements obtained by the devices can be filtered through an outlier filter. The outlier filter can be used to classify the edges in the graph as outliers or inliers, without the need of solving the complete robust PGO problem. Then only the identified loop closures can be considered in the robust PGO problem that tries to optimize the global graph.

[0015] Outlier filters can be designed in different ways. Graph-based methods, such as Pairwise Consistency Maximization (PCM) and its extension Group-k Consistency Maximization (GkCM), use graph-theoretic methods to identify a set of measurements, or edges, that are consistent with each other. Another option is to use a robust pose graph optimizer on a smaller problem, for example using GNC, to provide a robust frame estimation, which solves a robust pose averaging problem.

[0016] However, existing outlier filters often investigate inter-agent loop closures only pairwise or groupwise. Hence, only a subset of the measurements is used to identify outliers. This negatively impacts the filtering since not all available information is taken into account. Further, the computational cost of existing outlier filters is often too large to enable the outlier filters to be executed in real-time applications.

[0017] Hence, there is a need for improved outlier filtering in SLAM.

[0018] SUMMARY

[0019] An object of embodiments herein is to provide efficient outlier filtering in SLAM that does not suffer from the above issues, or at least where the above issues have been mitigated or reduced.

[0020] A particular object is to provide outlier filtering in SLAM that utilizes all available information related to the loop closures of interest.

[0021] A particular object is to provide outlier filtering in SLAM that can be executed in realtime applications.

[0022] According to a first aspect there is presented a method for map-based SLAM. A global map Q is composed of nodes representing poses of at least one device and edges relating the poses of one or several of the at least one device to each other. The method is performed by an image processing device. The method comprises obtaining new exteroceptive measurements from a device. The new exteroceptive measurements are to be added to the global map Q. The exteroceptive measurements yield new nodes and new edges in the global map Q. The method comprises identifying at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q. The method comprises combining the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q. The method comprises obtaining edge weights wtfor the edges of the at least one loop closure in the condensed global map Q. The method comprises updating the global map Q with new inliers and outliers as identified using the edge weights wt. The edges classified as inliers are added to the global map Q. The edges classified as outliers are removed from the global map Q.

[0023] Advantageously, this aspect provides outlier filtering in SLAM that does not suffer from the above issues. Advantageously, this aspect enables information of all inter-agent loop closures, and not just a subset, to be taken into account. In turn, this will generally increase the accuracy of the SLAM.

[0024] Advantageously, this aspect enables improvement over existing techniques for outlier filtering in SLAM with respect to accuracy or computational time, and provide tunable parameters such that it is possible to prioritize accuracy over computational time or vice versa.

[0025] Advantageously, by using initial pose estimates for priors obtained through robust PGO of the condensed map as priors in the global pose graph (i.e., the map) optimization, the maps of different devices can be anchored in a global reference frame, which enables a faster detection of any remaining outliers and therefore higher accuracy estimates.

[0026] Advantageously, instead of fixing the noise content to a user-defined level, i.e., using a fixed covariance matrix, the method of the first aspect enables noisy measurements to be propagated, and thus enables a near-exact representation of the noise to be maintained and further improve the accuracy of the outlier filter.

[0027] According to a second aspect there is presented an image processing device for mapbased SLAM. A global map Q is composed of nodes representing poses of at least one device and edges relating the poses of one or several of the at least one device to each other. The image processing device comprises processing circuitry. The processing circuitry is configured to cause the image processing device to obtain new exteroceptive measurements from a device. The new exteroceptive measurements are to be added to the global map Q. The exteroceptive measurements yield new nodes and new edges in the global map Q. The processing circuitry is configured to cause the image processing device to identify at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q. The processing circuitry is configured to cause the image processing device to combine the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q. The processing circuitry is configured to cause the image processing device to obtain edge weights wtfor the edges of the at least one loop closure in the condensed global map Q. The processing circuitry is configured to cause the image processing device to update the global map Q with new inliers and outliers as identified using the edge weights wt. The edges classified as inliers are added to the global map Q. The edges classified as outliers are removed from the global map Q.

[0028] According to a third aspect there is presented an image processing device for mapbased SLAM. A global map Q is composed of nodes representing poses of at least one device and edges relating the poses of one or several of the at least one device to each other. The image processing device comprises a first obtain module configured to obtain new exteroceptive measurements from a device. The new exteroceptive measurements are to be added to the global map Q. The exteroceptive measurements yield new nodes and new edges in the global map Q. The image processing device comprises an identify module configured to identify at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q. The image processing device comprises a combine module configured to combine the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q. The image processing device comprises a second obtain module configured to obtain edge weights wtfor the edges of the at least one loop closure in the condensed global map Q. The image processing device comprises an update module configured to update the global map Q with new inliers and outliers as identified using the edge weights wt. The edges classified as inliers are added to the global map Q. The edges classified as outliers are removed from the global map Q.

[0029] According to a fourth aspect there is presented a computer program for map-based SLAM. The computer program comprises computer code which, when run on processing circuitry of an image processing device, causes the image processing device to perform actions. One action comprises the image processing device to obtain new exteroceptive measurements from a device. The new exteroceptive measurements are to be added to the global map Q. The exteroceptive measurements yield new nodes and new edges in the global map Q. One action comprises the image processing device to identify at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q. One action comprises the image processing device to combine the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q. One action comprises the image processing device to obtain edge weights wtfor the edges of the at least one loop closure in the condensed global map Q. One action comprises the image processing device to update the global map Q with new inliers and outliers as identified using the edge weights wt. The edges classified as inliers are added to the global map Q. The edges classified as outliers are removed from the global map Q.

[0030] According to a fifth aspect there is presented a computer program product comprising a computer program according to the fourth aspect and a computer readable storage medium on which the computer program is stored. The computer readable storage medium could be a non-transitory computer readable storage medium.

[0031] Other objectives, features and advantages of the enclosed embodiments will be apparent from the following detailed disclosure, from the attached dependent claims as well as from the drawings.

[0032] Generally, all terms used in the claims are to be interpreted according to their ordinary meaning in the technical field, unless explicitly defined otherwise herein. All references to "a / an / the element, apparatus, component, means, module, step, etc." are to be interpreted openly as referring to at least one instance of the element, apparatus, component, means, module, step, etc., unless explicitly stated otherwise. The steps of any method disclosed herein do not have to be performed in the exact order disclosed, unless explicitly stated.

[0033] BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The inventive concept is now described, by way of example, with reference to the accompanying drawings, in which:

[0035] Fig. 1 is a schematic diagram illustrating a system according to embodiments;

[0036] Figs. 2 and 3 are flowcharts of methods according to embodiments; Figs. 4, 5, 6, and 7 illustrate examples of pose graphs according to embodiments;

[0037] Figs.8, 9, 10, and 11 show results according to embodiments;

[0038] Fig. 12 is a schematic diagram showing structural units of an image processing device according to an embodiment;

[0039] Fig. 13 is a schematic diagram showing functional modules of an image processing device according to an embodiment; and

[0040] Fig. 14 shows one example of a computer program product comprising computer readable storage medium according to an embodiment.

[0041] DETAILED DESCRIPTION

[0042] The inventive concept will now be described more fully hereinafter with reference to the accompanying drawings, in which certain embodiments of the inventive concept are shown. This inventive concept may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein; rather, these embodiments are provided by way of example so that this disclosure will be thorough and complete, and will fully convey the scope of the inventive concept to those skilled in the art. Like numbers refer to like elements throughout the description. Any step or feature illustrated by dashed lines should be regarded as optional.

[0043] Fig. 1 is a schematic diagram illustrating a system 100 according to embodiments. In Fig. 1(a) is illustrated a system 100a where localization and mapping of a partially known environment 150 is to be performed for a device 110. As a non-limiting example, the device 110 might be configured to implement some application that requires an accurate pose estimate. The device 110 comprises a sensor 120 for acquiring sensor data or other types of exteroceptive measurements of the partially known environment 150. The sensor might be a 2D camera (such as a monocular camera) or a 3D camera (such as a stereo camera), a RADAR module, a LIDAR module, etc., optionally in combination with one or more complementary sensors, such as an Inertial Measurement Unit (IMU), etc. The device 110 comprises a communications interface 130 for communicating the sensor data over a communications link 160 to an image processing device 140. The image processing device 140 is configured to perform map-based SLAM for the device 110. In Fig. 1(b) is illustrated a system 100b similar to the system 100a but where the image processing device 140 is provided in the device 110. The image processing device 140 is thus part of the device 110. There could be different types of devices, such as portable wireless devices, mobile stations, mobile phones, handsets, wireless local loop phones, user equipment (UE), smartphones, laptop computers, tablet computers, network-connectable sensor devices, network-connectable vehicles, gaming equipment, and Internet-of-things (loT) devices, such as mobile devices, etc.

[0044] As noted above, there is a need for improved outlier filtering in SLAM.

[0045] The embodiments disclosed herein therefore relate to techniques for map-based SLAM. In order to obtain such techniques there is provided an image processing device 140, 1200, 1300, a method performed by the image processing device 140, 1200, 1300, a computer program product comprising code, for example in the form of a computer program, that when run on an image processing device 140, 1200, 1300, causes the image processing device 140, 1200, 1300 to perform the method.

[0046] Fig. 2 is a flowchart illustrating embodiments of methods for map-based SLAM. A global map Q is composed of nodes representing poses of at least one device 110 and edges relating the poses of one or several of the at least one device 110 to each other. The methods are performed by the image processing device 140, 1200, 1300. The methods are advantageously provided as computer programs 1420.

[0047] S102: The image processing device 140, 1200, 1300 obtains new exteroceptive measurements from the device 110. The new exteroceptive measurements are to be added to the global map Q. The new exteroceptive measurements yield new nodes and new edges in the global map Q.

[0048] S104: The image processing device 140, 1200, 1300 identifies at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q. Sio6: The image processing device 140, 1200, 1300 combines the identified at least one loop closure with previously detected loop closures in the global map Q. This yields a condensed global map Q.

[0049] S108: The image processing device 140, 1200, 1300 obtains edge weights for the edges of the at least one loop closure in the condensed global map Q.

[0050] S112: The image processing device 140, 1200, 1300 updates the global map Q with new inliers and outliers as identified using the edge weights wt. The edges classified as inliers are added to the global map Q, and the edges classified as outliers are removed from the global map Q. Here, the inliers can thus be regarded as nonoutliers.

[0051] Embodiments relating to further details of map-based SLAM as performed by the image processing device 140, 1200, 1300 will now be disclosed with continued reference to Fig. 2.

[0052] There may be different ways to identify loop closures. In some aspects, if the new exteroceptive measurements from device i correspond to new or previously received exteroceptive measurements of device j #=i, then an inter-agent loop closure between the map of device i and the map of device j has been found. Hence, in some embodiments, the at least one loop closure is identified when the information extracted from the new exteroceptive measurements correspond to information extracted from new exteroceptive measurements of another device 110 or to information extracted from previously received exteroceptive measurements of the device 110 or this another device 110.

[0053] There may be different ways to determine when the identified at least one loop closure is to be combined with the previously detected loop closures in the global map Q. In some embodiments, the identified at least one loop closure is combined with the previously detected loop closures only upon the image processing device 140, 1200, 1300 having verified that a condition holds. There could be different such conditions. According to a first embodiment, the condition is fulfilled when a single loop closure has been identified. According to a second embodiment, the condition is fulfilled when at least two loop closures have been identified. Further aspects of how to determine when the identified at least one loop closure is to be combined with the previously detected loop closures in the global map Q will be disclosed below.

[0054] There could be different ways in which the identified at least one loop closure is combined with the previously detected loop closures. In some embodiments, combining the identified at least one loop closure with the previously detected loop closures comprises creating a global map, removing nodes from the new global map, and condensing edges in the new global map to yield the condensed global map Q.

[0055] In some aspects, the condensation is based on summarization of consecutive nodes that are between nodes involved in inter-agent loop closures and the removal of edges that are connected to only one node. In some aspects, the condensation is based on the exteroceptive measurements as well as on proprioceptive measurements. In this respect, the proprioceptive measurements pertain to measurements of internal properties of the devices no, such as motor speed, battery voltage, odometer measurements. Particularly, in some embodiments, combining the identified at least one loop closure with the previously detected loop closures comprises consolidating nodes that in the global map Q are located between nodes involved in the loop closure; and removing nodes that in the global map Q are connected to only one other node. This might also involve removing the first nodes in each map.

[0056] In some aspects, the map is condensed even further. Particularly, in some embodiments, combining the identified at least one loop closure with the previously detected loop closures further comprises obtaining, for each loop closure, a path including the loop that connects two nodes corresponding to initial poses of the device no, condensing each of the paths into a respective single edge between the initial pose nodes, and creating the condensed global map Q by connecting the nodes corresponding to the initial poses with the condensed edges.

[0057] Further aspects of how the identified at least one loop closure is combined with the previously detected loop closures will be disclosed below.

[0058] There can be different ways in which a loop closure is classified as an inlier (or an outlier). In some aspects, if a loop closure edge i has weight wtE [a, b] then it is classified as an inlier if wt> Jinand as an outlier otherwise. Here, ]inis a threshold value, which can be used to tune the outlier filter. That is, in some embodiments, each edge with an edge weight Wj G [a, b] is classified as an inlier if wt> Jinand otherwise as an outlier, where Jinis a threshold value.

[0059] There can be different ways to determine whether or not an inlier is to be kept in the global map or not. In some aspects, a loop closure edge i is only kept in the map if there exists at least NLCloops, where NLC> 1, in the global map that connects back to one of the nodes of the loop closure edge i via loop closure edges that have been identified as inliers according to the weights ivj. That is, in some embodiments, an edge classified as an inlier is only added to the global map Q when there exists at least one loop in the global map Q that connects back to one of the nodes of this edge.

[0060] In some aspects, estimates for the initial poses of the devices are obtained for interagent loop closures. Hence, in some embodiments, the image processing device 140, 1200, 1300 is configured to perform (optional) step S110.

[0061] S110: The image processing device 140, 1200, 1300 obtains estimates of initial poses of the at least one device 110.

[0062] In some aspects, for inter-agent loop closures, the estimates of the initial poses of the devices are used as prior constraints to the global map. Hence, in some embodiments, the estimates of initial poses are used as prior constraints when updating the global map Q with the new inliers and outliers to anchor maps of the at least one device 110 to a global reference frame.

[0063] There can be different actions taken by the image processing device 140, 1200, 1300 once having updating the global map Q in step S112. In some aspects, the updated global map Q is subjected to optimization. Therefore, in some embodiments, the image processing device 140, 1200, 1300 is configured to perform (optional) step S114.

[0064] S114: The image processing device 140, 1200, 1300 optimizes the global map Q using the information extracted from the new exteroceptive measurements.

[0065] There can be different ways in which the global map Q is optimized. In some examples, the global map Q is incrementally optimized or batch-style optimized. For example, the global map Q can be optimized with an incremental robust map optimizer such as DCS or Robust Incremental Smoothing and Mapping (riSAM), or in a batch fashion for example with a GNC-based solver.

[0066] One particular embodiment as performed by the image processing device 140, 1200, 1300 for map-based SLAM based on at least some of the above-disclosed embodiments will be disclosed next with reference to the flowchart of Fig. 3. This embodiment will, for the sake of simplicity, but without loss of generality, mostly be focused on inter-agent loop closures. Aspects to how the herein disclosed embodiments can be applied to intra-agent loop closures will be disclosed below. Also for the sake of simplicity, and without loss of generality, the global map Q will be represented by a global pose graph Q.

[0067] S201: Receive new measurements

[0068] In this step the image processing device 140, 1200, 1300 receives new exteroceptive measurements from one or more devices 110. These measurements should be added to a global pose graph Q. The new measurements will result in both new nodes and new edges in Q.

[0069] S202: Check for inter-agent loop closures

[0070] The new measurements are analyzed and compared to previously received measurements to identify any loop closures. If new measurements from device i correspond to new or previously received measurements of device j = i, then an interagent loop closure between the map of device i and the map of device j has been found.

[0071] If no inter-agent loop closure has been found, the server proceeds to step S206.

[0072] If an inter-agent loop closure has been found, different techniques can be used by the image processing device 140, 1200, 1300 to determine whether the outlier filter should be executed or not. Examples of three such techniques will be disclosed next.

[0073] According to a first example, the image processing device 140, 1200, 1300 executes the outlier filter in step S204 even if only one new inter-agent loop closure has been found. Still, all inter-agent loop closure edges are used in the condensation of the global graph in step S203 and not only the newly found one. One advantage of this example is that each new inter-agent loop closure is directly analyzed.

[0074] According to a second example, the new outlier loop closures are sent to the outlier filter for execution in step S204 only if at least x > 1 new inter-agent loop closures have been found since the last time the outlier filter was executed. This reduces the computational demand compared to the first example since the outlier filter is not run each time a single new inter-agent loop closure has been found.

[0075] According to a third example, the outlier filter is executed only when a certain condition holds. This can be formulated as follows. Let (X: 0) be a set-valued function parametrized by 0 that given a set returns values in the set {0,1}, i.e., depending on X the value of the function f(X) is either 0 or 1. Now assume that X c H, where H is the set of all inter-agent loop closures. Then the outlier filter is only executed if f(X) = 1 (and if f(X) = 0 the outlier filter is not executed). For example, let f(X; 0) = 1 if |X | > 0 and 0 otherwise and X represents the set of newly received inter-agent loop closures since the last execution of the outlier filter. This formulation recovers the first example with 0 = 1 and the second example with 0 = x. Furthermore, when changes are allowed to 0 then the first example and the second example can be combined as follows. Until f(X; 0) = 1 for the first time let 0 = x and then change it to 0 = 1. This means that the outlier filter is executed for the first time once x new inter-agent loop closures have been found, and then it is executed every time at least one new inter-agent loop closure has been found. There are many more potential choices of (X; 0) that would have different effects on the time of execution of the outlier filter.

[0076] S203: Condense global pose graph

[0077] The inter-agent loop closures detected in step S202 are combined with previously determined inter-agent loop closures, exteroceptive measurements, and proprioceptive measurements, of all devices that are connected via the inter-agent loop closures to create a new graph Q, which represents a condensed version of the global graph Q, since it has fewer nodes and edges. The condensation is based on the summarization of consecutive nodes that are between nodes involved in inter-agent loop closures and the removal of nodes that are connected to only one node, except the node corresponding to the initial pose of the devices. Examples of how Q can be determined in more detail will be disclosed below.

[0078] The way Q is obtained results in the least conservative and smallest sparse condensed graph, since the way the exteroceptive measurements are added does not lead to a loss in information. Since Q has fewer nodes and edges it might be faster to conduct robust PGO over Q than over Q, for example depending on if any auxiliary information, such as intra-agent loop closures, is used or not.

[0079] Different ways to further condense the graph, which leads to a pose graph G, will also be disclosed below. The pose graph G has as many nodes as there are devices and is a dense graph. Conducting robust PGO over Q will result in more conservative outlier estimates, but it is often faster to optimize over due to its small size. The speed up in computational time makes this graph suitable for real-time applications.

[0080] Step S204 represents the execution of the outlier filter. In this step, the condensed pose graph, as obtained in step S203, is used in a robust PGO to obtain weights, wbfor the inter-agent loop closures. These weights are used in step S205 to determine the outlier inter-agent loop closures and inlier inter-agent loop closures, as well as estimates for the initial poses of the devices, which can be used as prior constraints in step S206. The outlier filter is based on the condensed pose graph in the robust PGO.

[0081] Once the weights and the estimate of the initial poses are obtained, the image processing device 140, 1200, 1300 can update the global pose graph Q. The update comprises removing edges in the global pose graph that, based on the new measurements, have been identified as outliers. The update further comprises adding the new inlier (i.e., non-outlier) inter-agent loop closures to the global pose graph and updating the edges that have been identified to be inliers instead of outliers.

[0082] The initial pose estimates obtained from the pose graph optimization over the condensed graph for each device are added as prior constraints to the global pose graph with the corresponding covariance matrices. By adding the prior constraints, the pose graphs of the individual devices can be anchored in the global reference frame. This is especially helpful if the image processing device 140, 1200, 1300 sends summarized versions of the global graph back to the devices. In this case, the devices are better equipped to detect outliers without the help of the image processing device 140, 1200, 1300 when the devices observe outliers due to the devices’ knowledge of the global reference frame.

[0083] S206: Pose graph optimization over global pose graph

[0084] The image processing device 140, 1200, 1300 performs a robust pose graph optimization of its global graph Q. The robust pose graph optimization can be performed in an incremental fashion, for example with an incremental robust pose graph optimizer, such as DCS or riSAM, or in a batch fashion, for example, with a GNC-based solver.

[0085] If a batch optimizer is used, the batch optimization might not be solved for all measurements when a single new measurement is received. Therefore, the image processing device 140, 1200, 1300 might wait until M > 1 new measurements have been received before conducting global pose graph optimization. Another alternative is for the image processing device 140, 1200, 1300 to propagate the trajectories with the measurements, which does not require optimization, and execute a batch optimization when M loop closure measurements have been changed in the global pose graph.

[0086] Further aspect of how to condense the global pose graph will be disclosed next.

[0087] In general terms, condensation can be achieved by using the measurements of the devices that are connected through inter-agent loop closures and all available interagent loop closures to create a new smaller pose graph, while discarding the intraagent loop closures. By discarding the intra-agent loop closures, a pose graph that is sparser than the global pose graph Q is obtained. This sparse graph does not contain potential outliers that do not directly contribute to the device’s relative positioning. All available L inter-agent loop closures can be used, since some of them could have been wrongly identified as outliers in a previous iteration. Moreover, the sparse pose graph could be disconnected, since the inter-agent loop closures could, for example connect Device A and Device B with each other as well as Device C and Device D, but not Device A and Device C or Device D (where Device A, Device B, Device C, and Device D are examples of devices). If that is the case, the outlier filter could be split into several parallel threads on the image processing device 140, 1200, 1300, with one thread for each connected component of the condensed pose graph.

[0088] For ease of description but without loss of generality, the case of two devices will be described next. However, the approach is applicable to ND> 2 devices.

[0089] According to an example, the condensation of the global graph comprises three main steps; creating a new pose graph based on the measurements and the inter-agent loop closures, removing the nodes which are neither the nodes referring to the initial poses of the devices nor connected to more than one other node, and condensing the edges between two nodes that are part of an inter-agent loop closure edge by propagating their measurements.

[0090] An example of creating a new pose graph based on the measurements and the interagent loop closures is shown in Fig. 4, where two devices (hereinafter referred to as device A and device B, respectively), with four nodes each (with nodes Ao, Al, A2, and A3 belonging to a first device and nodes Bo, Bi, B2, and B3 belonging to a second device), are connected in a global pose graph 400 via two inter-agent loop closures; between nodes Al and Bi, and between nodes A2 and B3.

[0091] For the global pose graph 400, removing the nodes which are neither the nodes referring to the initial poses of the devices nor connected to more than one other node would mean to remove node A3 from the graph, whereas nodes Ao and Bo are maintained since nodes Ao and Bo refer to the initial poses of device A and device B, respectively.

[0092] With respect to the step of condensing the edges between two nodes that are part of an inter-agent loop closure edge by propagating their measurements, consider node B2 in Fig. 4. Assume that z12= z12® exp(e12) and z23= z23® exp(e23) are the measurements that created the edges between Bi and B2 and between B2 and B3, respectively, where z12and z23are the true measurement values, and e12and e23represent independent and uncorrelated zero-mean Gaussian random variables with covariance S12and S23, respectively. Here, the exponential map to go from a Lie algebra to its respective Lie group is denoted by exp(-) , and the composition operator for the d-dimensional Special Euclidean group, SE(d), where typically d E {2,3}, is denoted by ® . One way to condense these measurements is given by: zi3 =zi2 ®z23 = z12® exp(e12) ® z23® exp(e23) =zi2®z23®exp(AdjZ23e12+ e23), where Adjzdenotes the adjoint matrix, which maps a vector of the identity tangent space of a Lie group, such as SE(d), to the tangent space at z E SE(d).

[0093] Using these three steps, a reduced pose graph Q is obtained. This reduced pose graph Q has at most ND+ 2L nodes and 3L edges, where NDis the number of devices and L is the number of inter-agent loop closures. Hence, the size of the reduced pose graph Q is much smaller than the size of the corresponding global pose graph and, therefore, faster to optimize over. Furthermore, the reduced pose graph Q is also a sparse graph, which enables the use of efficient pose graph optimization algorithms. Applying these three steps to the global pose graph 400 in Fig. 3, results in the reduced pose graph 500 in Fig. 5 where thus nodes A3 and B2 have been removed.

[0094] It is, optionally, possible to further condense the generated reduced pose graph Q. Condensing the reduced pose graph Q further would result in an even smaller pose graph, which could speed up the robust PGO even more.

[0095] One example of how to further condense the generated reduced pose graph Q will be disclosed next.

[0096] First, for each loop closure, a path including the respective loop closure and measurements that connects the two nodes corresponding the initial poses of the devices involved in the inter-agent loop closure are obtained. For example, the reduced pose graph 500 has two paths connecting nodes Ao and Bo, as illustrated in the pose graphs 600a, 600b in Fig. 6. The L paths, one path for each inter-agent loop closure, can be condensed into a single edge between the initial pose nodes similar to how the measurements are condensed above. For example, the paths in Fel! Hitar inte referenskalla.Fig. 6 can be condensed to the edges shown in the left part of Fig. 7. A new graph 700 can then be created by connecting the initial pose nodes with the obtained condensed edges. An example of how this graph could look like for the pose graph 400 is shown in the right part of Fig. 7. Using these three additional steps, the resulting graph g has NDnodes, i.e., one node for the initial pose of each device, and L edges, i.e., one for each inter-agent loop closure. One benefit of g is that it is a much smaller graph than the original global pose graph, albeit denser, such that conducting pose graph optimization is even faster.

[0097] Moreover, both Q and Q can in theory be built in an incremental fashion, such that for each new inter-agent loop closures, the pose graphs do not need to be re-built from scratch. For example, a new inter-agent loop closure will lead to a single new edge in Q, which can simply be added without changing the rest of the pose graph.

[0098] Further aspect of how to perform the robust pose graph estimation over the condensed graph will be disclosed next.

[0099] For robust pose graph estimation, the image processing device 140, 1200, 1300 uses a condensed graph, for example, g or g, in a robust pose graph optimization problem to determine the outlier loop closures as well as the estimate of the initial poses of the devices, which are used as prior constraint. Since the condensed graph is still a pose graph, robust pose graph optimization methods, such as GNC and DCS can be used. Further, since G has fewer nodes and edges than the global pose graph, an optimization method, for example, GNC, can be used to optimize over this smaller graph to obtain an estimate of the poses as well as an estimate of the edge weights. The edge weights can then be used to determine if one edge is considered an outlier or not. As noted above, performing robust pose graph optimization over the condensed graph might give faster results than optimizing over g directly (for example depending on if any auxiliary information, such as intra-agent loop closures, is used or not), since there are fewer nodes and edges in the condensed graph.

[0100] The resulting weights can then be used to identify outlier and inlier edges. In case PGO, such as GNC and DCS, is used to filter the outliers, also estimates for the initial poses of the devices are obtained for free with the filtering. For example, if an interagent loop closure i has weight wtE [a, b] then it is classified as an inlier if wt> Jinand as an outlier otherwise. Here, ]inis a user-defined threshold, which can be used to tune the outlier filter. An example of how to choose Jinis by creating a receiver operator characteristic (ROC) curve and using the threshold corresponding to a desired area under the ROC curve. Some robust pose graph optimizers, such as GNC, return weights wtE {0,1}, such that the need for ]infalls away.

[0101] The detected inlier edges can also be partially removed as follows. An inlier interagent loop closure edge i might only be kept in Q if there exists at least NLCloops in the global graph that connects back to one of the nodes of the inlier inter-agent loop closure i, where all inter-agent loop closures in the loop have been identified as an inlier and each of the inter-agent loop closures appears only once in the loop. If that is not fulfilled for an inlier inter-agent loop closure it might be removed from Q.

[0102] Further aspect of how to update the global pose graph will be disclosed next.

[0103] Once the condensed graphs have been generated and the robust pose graph optimization on the condensed graphs has returned the weights to identify inlier inter-agent loop closures and outlier inter-agent loop closures and the estimates for the initial poses of each device, the global graph Q can be updated with the new inliers and outliers. The prior constraints can be added based on the initial pose estimates of each device. This means that the edges considered inliers are added to the global graph, whilst the outlier edges can be removed from the global graph.

[0104] The addition of a virtual prior measurement to each of the devices in the global graph introduces a new constraint for each device in the global pose graph. The virtual prior measurement of the devices is based on the estimate of the initial poses coming from the PGO of the condensed graph. From the pose graph optimization of Q or Q, an estimate for the initial poses is obtained, for example, the nodes Ao and Bo in the above example. These estimates can be used to anchor the measurements of the devices in a global reference frame via virtual prior constraint, which improves the estimation accuracy.

[0105] Aspects of intra-agent loop closures will be disclosed next.

[0106] Odometry can be used as a backbone for identifying intra-agent loop closures. However, the herein disclosed techniques for outlier filtering can also be applied to identifying intra-agent loop closures. In fact, the identification of inter- and intra- agent loop closures in the image processing device 140, 1200, 1300 can be parallelized. For example, it is possible to create Q as described above for the inter- agent loop closures, and also create a similar graph for each device based on the device’s odometry and intra-agent loop closures. Then the outlier filter can be executed in parallel for these ND+ 1 pose graphs.

[0107] Aspects of applications of the herein disclosed embodiments to distributed C-SLAM algorithms will be disclosed next.

[0108] In distributed C-SLAM algorithms it is common to perform the robust frame estimation in pairs. For example, everytime two devices encounter each other, or are in communication range of each other, the devices might exchange parts or all of their pose graphs and perform a robust frame estimation to align their frames to a global pose graph. In case this procedure is performed pairwise, the herein disclosed embodiments can be applied also to a distributed setup as well as in Fig. 1(b), where one of the two devices implement the image processing device 140, 1200, 1300, and sends the inlier edges and prior constraints to the other device.

[0109] A performance comparison herein disclosed embodiments to existing technology will be disclosed next.

[0110] In Fig. 8 and Fig. 9 are shown boxplots of the mean and median collaborative error of the global pose graph for DCS without an outlier filter and using DCS with three different outlier filters, which are PCM and the two proposed methods, over different amounts of outliers. In more details, in these figures the average trajectory error (ATE) of standard DCS without any outlier filter to is compared to DCS with an outlier filter. Three different outlier filters are considered, namely PCM and two proposed approaches (using Q and using Q, respectively). The pose estimation was performed over 10 randomly generated datasets with two devices. It can be observed that an outlier filter is necessary for DCS and that in the median all outlier filters perform well, i.e., they are able to correctly estimate the frame. However, it can also be observed that the mean error for PCM has a larger spread, which shows that it performs worse on average. Furthermore, it can be observed that the results for Q are consistently better than for Q.

[0111] Next, a comparison is made with respect to how well the herein disclosed embodiments scale with the number of devices. Here, the number of nodes in the global pose graph is fixed to 5000 and the outlier ratio is fixed to 0.3. The number of devices NDE {2, 3, 4, 5} is varied and the performance of DCS for different outlier filters is compared, and the results are shown in the box plots of Fig. io and Fig. 11. Fig. io and Fig. n provide a comparison of the influence of the number of devices on the trajectory error of DCS with PCM and using outlier filters according to two proposed approaches (using Q and using Q, respectively). In Fig. io and Fig. n can be observed that if ND< 3 the herein disclosed embodiments yield a higher accuracy than PCM, but for more devices the accuracy decreases compared to PCM. The general trend for the iteration times does not change though. That is, the time to solve the DCS with Q increases, while the PCM and Q have a lower iteration. PCM in a C-SLAM setup is usually used for only two devices at a time. Hence, the two proposed approaches are able to take at least one more device into account. This means that the robust frame estimation can be performed for two or three devices at a time and achieving a better performance than PCM.

[0112] It has thus been demonstrated that the herein disclosed embodiments surpass state- of-the-art outlier filters in terms of accuracy, computational time, and the number of devices.

[0113] Fig. 12 schematically illustrates, in terms of a number of structural units, the components of an image processing device 1200 according to an embodiment. Processing circuitry 1210 is provided using any combination of one or more of a suitable central processing unit (CPU), multiprocessor, microcontroller, digital signal processor (DSP), etc., capable of executing software instructions stored in a computer program product 1410 (as in Fig. 14), e.g. in the form of a storage medium 1230. The processing circuitry 1210 may further be provided as at least one application specific integrated circuit (ASIC), or field programmable gate array (FPGA).

[0114] Particularly, the processing circuitry 1210 is configured to cause the image processing device 1200 to perform a set of operations, or steps, as disclosed above. For example, the storage medium 1230 may store the set of operations, and the processing circuitry 1210 may be configured to retrieve the set of operations from the storage medium 1230 to cause the image processing device 1200 to perform the set of operations. The set of operations may be provided as a set of executable instructions. Thus the processing circuitry 1210 is thereby arranged to execute methods as herein disclosed. The storage medium 1230 may also comprise persistent storage, which, for example, can be any single one or combination of magnetic memory, optical memory, solid state memory or even remotely mounted memory. The image processing device 1200 may further comprise a communications (comm.) interface 1220 at least configured for communications with other entities, functions, nodes, and devices, as in Fig. 1. As such the communications interface 1220 may comprise one or more transmitters and receivers, comprising analogue and digital components. The processing circuitry 1210 controls the general operation of the image processing device 1200, e.g. by sending data and control signals to the communications interface 1220 and the storage medium 1230, by receiving data and reports from the communications interface 1220, and by retrieving data and instructions from the storage medium 1230. Other components, as well as the related functionality, of the image processing device 1200 are omitted in order not to obscure the concepts presented herein.

[0115] Fig. 13 schematically illustrates, in terms of a number of functional modules, the components of an image processing device 1300 according to an embodiment. The image processing device 1300 of Fig. 13 comprises a number of functional modules; a first obtain module 1310 configured to perform step S102, an identify module 1320 configured to perform step S104, a combine module 1330 configured to perform step S106, a second obtain module 1340 configured to perform step S108, and an update module 1360 configured to perform step S112. The image processing device 140,

[0116] 1200. 1300 of Fig. 13 may further comprise a number of optional functional modules, such as any of a third obtain module 1350 configured to perform step S110, and an optimize module 1370 configured to perform step S114. In general terms, each functional module 1310:1370 may in one embodiment be implemented only in hardware and in another embodiment with the help of software, i.e., the latter embodiment having computer program instructions stored on the storage medium 1230 which when run on the processing circuitry makes the image processing device

[0117] 140. 1200. 1300 perform the corresponding steps mentioned above in conjunction with Fig 13. It should also be mentioned that even though the modules correspond to parts of a computer program, they do not need to be separate modules therein, but the way in which they are implemented in software is dependent on the programming language used. Preferably, one or more or all functional modules 1310:1370 maybe implemented by the processing circuitry 1210, possibly in cooperation with the communications interface 1220 and / or the storage medium 1230. The processing circuitry 1210 may thus be configured to from the storage medium 1230 fetch instructions as provided by a functional module 1310:1370 and to execute these instructions, thereby performing any steps as disclosed herein.

[0118] The image processing device 140, 1200, 1300 may be provided as a standalone device or as a part of at least one further device. For example, the image processing device 140, 1200, 1300 may be provided in the device 110, as in Fig. 1(b). Alternatively, functionality of the image processing device 140, 1200, 1300 may be distributed between at least two devices. A first portion of the instructions performed by the image processing device 140, 1200, 1300 may be executed in a first device, and a second portion of the of the instructions performed by the image processing device 140, 1200, 1300 may be executed in a second device; the herein disclosed embodiments are not limited to any particular number of devices on which the instructions performed by the image processing device 140, 1200, 1300 may be executed. Hence, the methods according to the herein disclosed embodiments are suitable to be performed by an image processing device 140, 1200, 1300 residing in a cloud computational environment. Therefore, although a single processing circuitry 1210 is illustrated in Fig. 12 the processing circuitry 1210 may be distributed among a plurality of devices, or nodes. The same applies to the functional modules 1310:1370 of Fig. 13 and the computer program 1420 of Fig. 14.

[0119] Fig. 14 shows one example of a computer program product 1410 comprising computer readable storage medium 1430. On this computer readable storage medium 1430, a computer program 1420 can be stored, which computer program 1420 can cause the processing circuitry 1210 and thereto operatively coupled entities and devices, such as the communications interface 1220 and the storage medium 1230, to execute methods according to embodiments described herein. The computer program 1420 and / or computer program product 1410 may thus provide means for performing any steps as herein disclosed.

[0120] In the example of Fig. 14, the computer program product 1410 is illustrated as an optical disc, such as a CD (compact disc) or a DVD (digital versatile disc) or a Blu-Ray disc. The computer program product 1410 could also be embodied as a memory, such as a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), or an electrically erasable programmable read-only memory (EEPROM) and more particularly as a non-volatile storage medium of a device in an external memory such as a USB (Universal Serial

[0121] Bus) memory or a Flash memory, such as a compact Flash memory. Thus, while the computer program 1420 is here schematically shown as a track on the depicted optical disk, the computer program 1420 can be stored in any way which is suitable for the computer program product 1410. The inventive concept has mainly been described above with reference to a few embodiments. However, as is readily appreciated by a person skilled in the art, other embodiments than the ones disclosed above are equally possible within the scope of the inventive concept, as defined by the appended patent claims.

Claims

CLAIMS1. A method for map-based Simultaneous Localization and Mapping, wherein a global map Q is composed of nodes representing poses of at least one device (no) and edges relating the poses of one or several of the at least one device (no) to each other, wherein the method is performed by an image processing device (140, 1200, 1300), and wherein the method comprises: obtaining (S102) new exteroceptive measurements from a device (no), wherein the new exteroceptive measurements are to be added to the global map Q, the new exteroceptive measurements yielding new nodes and new edges in the global map Q; identifying (S104) at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q; combining (S106) the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q; obtaining (S108) edge weights wtfor the edges of the at least one loop closure in the condensed global map Q; and updating (S112) the global map Q with new inliers and outliers as identified using the edge weights wt, wherein the edges classified as inliers are added to the global map Q, and the edges classified as outliers are removed from the global map Q.

2. The method according to claim 1, wherein the at least one loop closure is identified when the information extracted from the new exteroceptive measurements correspond to information extracted from new exteroceptive measurements of another device (110) or to information extracted from previously received exteroceptive measurements of the device (110) or said another device (110).

3. The method according to claim 1 or 2, wherein the identified at least one loop closure is combined with the previously detected loop closures upon the image processing device (140, 1200, 1300) having verified that a condition holds.4- The method according to claim 3, wherein the condition is fulfilled when a single loop closure has been identified.

5. The method according to claim 3, wherein the condition is fulfilled when at least two loop closures have been identified.

6. The method according to any of claims 1 to 5, wherein combining the identified at least one loop closure with the previously detected loop closures comprises creating a global map, removing nodes from the new global map, and condensing edges in the new global map to yield the condensed global map Q.

7. The method according to any of claims 1 to 6, wherein combining the identified at least one loop closure with the previously detected loop closures comprises:- consolidating nodes that in the global map Q are located between nodes involved in the loop closure; and- removing nodes that in the global map Q are connected to only one other node.

8. The method according to any of claims 1 to 7, wherein combining the identified at least one loop closure with the previously detected loop closures further comprises:- obtaining, for each loop closure, a path including the loop that connects two nodes corresponding to initial poses of the device (110);- condensing each of the paths into a respective single edge between the initial pose nodes; and- creating the condensed global map Q by connecting the nodes corresponding to the initial poses with the condensed edges.

9. The method according to any of claims 1 to 8, wherein each edge with an edge weight Wi E [a, b] is classified as an inlier if wt> Jinand otherwise as an outlier, where Jinis a threshold value.

10. The method according to any of claims 1 to 9, wherein an edge classified as an inlier is only added to the global map Q when there exists at least one loop in the global map Q that connects back to one of the nodes of said edge.

11. The method according to any of claims 1 to 10, wherein the method further comprises: obtaining (Siio) estimates of initial poses of the at least one device (no).

12. The method according to claim n, wherein the estimates of initial poses are used as prior constraints when updating the global map Q with the new inliers and outliers to anchor maps of the at least one device (no) to a global reference frame.

13. The method according to any of claims 1 to 12, wherein the method further comprises: optimizing (S114) the global map Q using the information extracted from the new exteroceptive measurements.

14. The method according to claim 13, wherein the global map Q is incrementally optimized or batch-style optimized.

15. An image processing device (140, 1200) for map-based Simultaneous Localization and Mapping, wherein a global map Q is composed of nodes representing poses of at least one device (no) and edges relating the poses of one or several of the at least one device (110) to each other, the image processing device (140, 1200) comprising processing circuitry (1210), the processing circuitry being configured to cause the image processing device (140, 1200) to: obtain new exteroceptive measurements from a device (no), wherein the new exteroceptive measurements are to be added to the global map Q, the new exteroceptive measurements yielding new nodes and new edges in the global map Q; identify at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q; combine the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q;obtain edge weights wtfor the edges of the at least one loop closure in the condensed global map Q; and update the global map Q with new inliers and outliers as identified using the edge weights wt, wherein the edges classified as inliers are added to the global map Q, and the edges classified as outliers are removed from the global map Q.

16. An image processing device (140, 1300) for map-based Simultaneous Localization and Mapping, wherein a global map Q is composed of nodes representing poses of at least one device (110) and edges relating the poses of one or several of the at least one device (110) to each other, the image processing device (140, 1300) comprising: a first obtain module (1310) configured to obtain new exteroceptive measurements from a device (110), wherein the new exteroceptive measurements are to be added to the global map Q, the new exteroceptive measurements yielding new nodes and new edges in the global map Q; an identify module (1320) configured to identify at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q; a combine module (1330) configured to combine the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q; a second obtain module (1340) configured to obtain edge weights wtfor the edges of the at least one loop closure in the condensed global map Q; and an update module (1360) configured to update the global map Q with new inliers and outliers as identified using the edge weights wt, wherein the edges classified as inliers are added to the global map Q, and the edges classified as outliers are removed from the global map Q.

17. The image processing device 140, 1200, 1300 according to claim 15 or 16, further being configured to perform the method according to any of claims 2 to 14.

18. A computer program (1420) for map-based Simultaneous Localization and Mapping, wherein a global map Q is composed of nodes representing poses of at least one device (110) and edges relating the poses of one or several of the at least one device (110) to each other, the computer program comprising computer code which, when run on processing circuitry (1210) of an image processing device (140, 1200), causes the image processing device (140, 1200) to: obtain (S102) new exteroceptive measurements from a device (110), wherein the new exteroceptive measurements are to be added to the global map Q, the new exteroceptive measurements yielding new nodes and new edges in the global map Q; identify (S104) at least one loop closure for the new nodes and the new edges in the global map Q by comparing information extracted from the new exteroceptive measurements to information extracted from existing exteroceptive measurements for the global map Q; combine (S106) the identified at least one loop closure with previously detected loop closures in the global map Q, yielding a condensed global map Q; obtain (S108) edge weights wtfor the edges of the at least one loop closure in the condensed global map Q; and update (S112) the global map Q with new inliers and outliers as identified using the edge weights wt, wherein the edges classified as inliers are added to the global map Q, and the edges classified as outliers are removed from the global map Q.

19. A computer program product (1410) comprising a computer program (1420) according to claim 18, and a computer readable storage medium (1430) on which the computer program is stored.

Images