Multi-scale Perception and Path Planningon Probabilistic Obstacle Maps

Florian Hauer1 Abhijit Kundu2 James M. Rehg3 Panagiotis Tsiotras4

Georgia Institute of Technology, Atlanta, GA 30332-0150

Abstract— We present a path-planning algorithmthat leverages a multi-scale representation of theenvironment. The algorithm works in n dimensions.The information of the environment is stored in a treerepresenting a recursive dyadic partitioning of thesearch space. The information used by the algorithm isthe probability that a node of the tree corresponds toan obstacle in the search space. The complexity of theproposed algorithm is analyzed and its completenessis shown.

I. IntroductionThe success of any path-planning algorithm depends

on the perception algorithm used to localize the agentand create the map of the environment, including theobstacles. Often, it is advantageous to hierarchicallyorganize the collected data about the environment. Thisis motivated by the following two observations: First,processing all collected data at the finest resolutionmay be computationally prohibitive, especially for smallrobotic platforms with limited on-board computationalresources. In addition, processing all collected informa-tion at the same temporal and spatial scale may noteven be necessary for path-planning purposes, as pathfeasibility is primarily determined by the obstacles in thevicinity of the vehicle; far away obstacles, on the otherhand, affect longer-term objectives, such as exploring theenvironment, reaching the goal state, etc. Second, thismulti-scale hierarchy is often brought about by the per-ception system itself. Indeed, depending on the sensors,the collected information about the environment is rarelyuniformly accurately known, and collected informationis often represented by probability values modeling mea-surement uncertainty [21].

Different approaches have been studied for solvingpath-planning problems using multi-scale maps. Top-down approaches consist of finding a path at the coarsestresolution level and subsequently progressively increasingits resolution [8], [16]. Bottom-up approaches solve theproblem at each node at the finest resolution level and

1PhD candidate, School of Aerospace Engineering,Email:fhauer3@gatech.edu

2PhD candidate, College of Computing andInstitute for Robotics and Intelligent Machines,Email:abhijit.kundu@gatech.edu

3Profesor, College of Computing and Institute for Robotics andIntelligent Machines, Email:rehg@cc.gatech.edu

4Professor, School of Aerospace Engineering and Institute forRobotics and Intelligent Machines, Email:tsiotras@gatech.edu

Support for this work has been provided by ARO MURI awardW911NF-11-1-0046, ONR award N00014-13-1-0563 and the IntelScience and Technology Center in Embedded Computing

combine the results at different resolution levels. Thisapproach yields optimality, but requires knowing andprocessing the entire map at the finest resolution [13],which may be computationally expensive. Holte [6] de-scribes an approach that includes bottom-up and top-down analysis of the data by generalizing the multi-scale problem to a multi-abstraction problem. From thefinest information, an abstraction of the problem can beconstructed such that the topology of the original searchspace is maintained in the abstraction, and the size ofthe problem is reduced. Repeating the process leads toa sequence of abstractions topologically similar to theoriginal search space, but with a decreasing size of theresulting abstract search spaces. Another approach is touse information at different resolutions simultaneously.This idea is explored in [3], where areas near the currentvehicle are represented accurately, while farther-awayareas are coarsely-encoded by using a transformationon the wavelet coefficients. The approach is shown tobe complete and very fast. A similar approach is usedto create a local map in [1], but quadtrees are usedinstead of wavelets and only local planning is done. Otheralgorithms have been developed using multi-resolutionmaps, but they are often applied to a given non-uniformgrid, without using the information at different resolutionscales for the same region of the search space [4], [17].

In this paper we propose an extension of the algorithmintroduced in [3] that is applicable for solving path-planning problems in n-dimensional search spaces. Thealgorithm uses an approach similar to [1] to create a localrepresentation of the environment. We also propose anew way to incorporate the collected information thattakes into account the spatial coherency of the data usinga conditional random field (CRF) [12]. The algorithmdirectly utilizes data-structures created from state-of-the-art perception algorithms, such as octrees [7] orwavelet-coefficient trees [22], which allows the proposedpath-planning algorithm to be easily incorporated insuch perception algorithms, thus tightly integrating theperception and execution layers in autonomous roboticsystems.

II. Problem FormulationA. Multiresolution World Representation

The environment W ⊂ Rd is assumed to be a d-dimensional grid world contained within a hypercube ofside length 2`. The world W is not perfectly known,but an estimate of the probability label of each cell ismaintained in a tree T = (N ,R) created from a set

of measurements D. Each nk,p ∈ N has the followingproperties:• It represents a hypercube H(nk,p) ⊆ W of side

length 2k and volume 2dk centered at p.• It is at depth `− k in T .• Its children are nk−1,qi , i ∈ [1, 2d] where qi = p +

2k−2ei and where ei is each of the 2d (d-dimensional)vectors generated by [±1,±1, . . . ,±1]. The hyper-cubes associated with the children of the node nk,p

induce a dyadic partitioning of H(nk,p), as followsH(nk,p) =

⋃2di=1 H(nk−1,qi).

• The node is a leaf of T if it has no children.• The value V (nk,p) of node nk,p is the probability

that the label of H(nk,p) is obstacle if it is a leaf, orthe average of the values of its children if the nodenk,p is not a leaf. That is,

V (nk,p) ={P (onk,p = obstacle|D), if nk,p is a leaf,1

2d∑2d

i=1 V (nk−1,qi), otherwise,

where onk,p ∈ {obstacle, free space} is the binaryoccupancy label associated with H(nk,p) and D isthe set of measurements used to create T .

B. The Path-Planning ProblemTwo nodes of T are neighbors if their corresponding

hypercubes share a face, specifically, their intersection isa hypercube of dimension d−1. A necessary and sufficientcondition for two nodes nk1,p1 and nk2,p2 to be neighborsis that both of the following two conditions are satisfied:a) ‖p1 − p2‖∞ = 2k1−1 + 2k2−1,b) There exists a unique i ∈ [1, d], such that |(p1 −

p2)i| = 2k1−1 + 2k2−1, where (p1 − p2)i is the ith

component of the vector.We define a path π = (nk1,p1 , nk2,p2 , . . . , nkN ,pN ) in

N to be a sequence of nodes nki,pi ∈ N , each atcorresponding position pi and depth `−ki in T , such thattwo consecutive nodes of the sequence are neighbors. Apath is a finest information path (FIP) if all its nodesare leafs of T . Note that a finest information path maycontain nodes that are not of unit size. Given ε ∈ [0, 1),a node nk,p ∈ N is an ε-obstacle if V (nk,p) ≥ 1− 2−dkε.A path π is ε-feasible if none of its nodes are ε-obstacles.

Proposition 1. If a node in T is an ε-obstacle, then allleaf nodes descendant from this node are also ε-obstacles.

Proof. Let nk,p ∈ N and let nmi,qi , where i ∈ [1, L] bethe descendant leaf nodes of nk,p. From the definition ofV (nk,p) it can be easily shown that

V (nk,p) = 12dk

L∑i=1

2dmiV (nmi,qi). (1)

Since the leaf nodes descendent from nk,p form a parti-tion of H(nk,p), it follows that 2dk =

∑Li=1 2dmi .

Suppose now that nk,p is an ε-obstacle and assume, onthe contrary, that there exists nmj ,qj , j ∈ [1, L] such thatnmj ,qj is not an ε-obstacle, i.e., assume that V (nmj ,qj ) <1 − 2−dmjε. Since nk,p is an ε-obstacle, V (nk,p) ≥ 1 −

2−dkε. It follows, from (1), that

V (nk,p) = 12dk

L∑i=1,i6=j

2dmiV (nmi,qi) + 2dmjV (nmj ,qj )

< 1− 2d(mj−k) + 2d(mj−k)(1− 2−dmjε) < 1− 2−dkε.

leading to a contradiction.

Using a threshold for the ε-obstacle that depends onthe size of the node extends the work in [3] to any valueof ε ∈ [0, 1) instead of only small enough ε.

III. Proposed Algorithm: The multi-scale PathPlanning (MSPP) Algorithm

The overall idea of the proposed multi-scale PathPlanning (MSPP) algorithm is to iteratively solve smallerproblems instead of solving the original problem at once.Starting at iteration i = 0 from nk0,p0 = nstart, thealgorithm uses T to create a local representation of theenvironment encoded in the reduced graph Gi = (Vi, Ei),where Vi ⊆ N , as shown in Figure 1. The vertices of Gi

are a collection of the nodes of T forming a partition ofthe search space, with fine resolution around the currentnode, say nki,pi , at iteration i, and with progressivelycoarser resolutions away from nki,pi (see Figure 1). Theresolution levels and the horizon of each resolution levelare controlled by the parameters ` and α (see Section IV-C). The graph Gi is thus a spatial representation of W,as opposed to T , which is a hierarchical representationof W.

In order to avoid confusion, henceforth we will referto the elements of Gi as vertices, and the elements of Tas nodes. Every vertex corresponds to a node, but theconverse is not true. The notation node(v) ∈ N will beused to refer to the node in N corresponding to vertexv ∈ Vi.

(a) (b) (c)

Fig. 1: (a) Example environmentW with the current cell,nki,pi , shown in red. Obstacles are drawn with solid bluecolor; (b) Corresponding tree T ; (c) Reduced graph Gi

around nki,pi in red and corresponding space partition.

A sequence of path-planning problems are solved ineach graph Gi, (i = 0, 1, 2, . . .) as follows. The shortestpath πgoal

i = (vistart, π

i1, π

i2, π

i3, . . . , v

igoal) from vi

start tovi

goal is found in Gi, where vistart and vi

goal are thevertices in Gi that are the (unique) ancestors of nki,pi

and ngoal in T , πik ∈ Vi and (πi

k, πik+1) ∈ Ei. Let

nki+1,pi+1 = node(πi1). Then vi+1

start is set to nki+1,pi+1 andthe process is repeated until πi

1 = vigoal for some i ≥ 0.

At iteration i, let the partial path candidate πistart =

(nk0,p0 , nk1,p1 , . . . , nki,pi) be the path constructed by thealgorithm thus far. By construction (see Proposition 5),

the vertices of Gi neighboring nki,pi are necessarily leafsof T and hence are finest information nodes. It followsthat πi

start is a FIP, and hence at termination the pathπ = (nk0,p0 , nk1,p1 , . . . , ngoal) is also a FIP.

A. Reduced Graph ConstructionThe vertices of Gi are selected recursively

starting from the root of T using the func-tion GetReducedGraphVertices. A node nk,p ∈ Nis selected to be included as a vertex in Gi if all of thefollowing are true:a) The node nk,p is a leaf, or ‖p−pi‖2−

√d

2 2ki > α2k,where pi and ` − ki are the position and the depthof the current node nki,pi , respectively, and α > 0 isa parameter.

b) The node nk,p is not an ε-obstacle.c) The node nk,p does not contain a part of the partial

path candidate πistart = (nk0,p0 , nk1,p1 , . . . , nki,pi),

that is, H(nk,p) ∩H(nkj ,pj ) = ∅, j = 0, 1, . . . , i.When a node is not selected, its children are then

considered, and the process repeats itself until all nodesof T have been processed. The selected nodes form apartition of the search space from which we have removedε-obstacles (condition b)) and all nodes correspondingto the current partial path candidate since we want aloopless ε-feasible solution (condition c)). Every pair ofnodes selected is tested against the neighborhood tests,and edges are created between neighboring nodes.

Function GetReducedGraphVerticesData: Node nk,p,Vertex list vertices, Current node

nki,pi

1 if (‖p− pi‖2 −√

d2 2ki ≥ α2k OR isLeaf(nk,p)) AND

doesNotContainPath(nk,p) then2 if cost(nk,p)< M then3 vertices← vertices ∪ nk,p

4 else5 foreach nm,q child of nk,p do6 GetReducedGraphVertices(nm,q,vertices,nki,pi)

B. Backtracking AlgorithmThe proposed MSPP algorithm is a backtracking al-

gorithm [18] and is summarized in Algorithm 1. At eachiteration i, the algorithm creates the reduced graph Gi asdetailed in Section III-A. It then evokes a search to findthe shortest path from the vertex vi

start correspondingto the current node nki,pi = node(vi

start) to the vertexvi

goal corresponding to the goal node ngoal. To guaranteecompleteness of the MSPP algorithm, the invoked searchalgorithm must return a solution, if one exists. The A∗algorithm [5] is used in our implementation. Once thealgorithm reaches the goal, it terminates and returns thesolution path stored in πi

start. Note that if the algorithmbacktracks when nki,pi = nstart then it will have triedevery possible path without finding a solution, in whichcase it reports failure.

Algorithm 1: The MSPP AlgorithmData: Tree T , Start node nstart, Goal node ngoalResult: ε-feasible FIP from nstart to ngoal or failure

1 i← 0, nki,pi ← nstart,π0start ← [nki,pi ];

2 visits(nk,p)← ∅,∀nk,p;3 while true do4 (Gi, v

istart, v

igoal)←ReducedGraph(T ,nki,pi);

5 πgoali ←SP(Gi, v

istart, vgoal,i,visits(nki,pi));

6 if exists(πgoali ) then

7 nki+1,pi+1 ←node(firstElement(πgoali ));

8 visits(nki,pi)←visits(nki,pi)∪nki+1,pi+1 ;9 πi+1

start ← [πistart nki+1,pi+1 ];

10 if nki+1,pi+1 =ngoal then11 return πi+1

start;12 else13 visits(nki,pi)← ∅;14 πi+1

start=removeLastElement(πistart);

15 if πi+1start = ∅ then

16 Report failure;17 else18 nki+1,pi+1 ←lastElement(πi+1

start);

19 i← i+ 1;20 Report failure;

In our implementation the cost of traversing a node ischosen as

cost(nk,p) = 2dk(λ1V (nk,p) + λ2) (2)

where λ1, λ2 ∈ (0, 1]. Other costs can be chosen depend-ing on the problem at hand. The cost in (2) takes intoaccount the probability that the node nk,p is an obstaclewith weight λ1, and it adds the length of the pathwith weight λ2, scaled by the volume of the hypercubecorresponding to the node.

It is clear that it is desirable to detect non-promisingpartial path candidates as soon as possible in order tobacktrack early on, and avoid spending computationalresources completing a partial path candidate that willnot lead to a valid solution. The following corollary guar-antees the existence of a path in Gi when there exists anε-feasible FIP contained in the space represented by Gi.In the following, Ri denotes the region in W representedby the vertices of Gi, that is, Ri =

⋃vi∈Vi H(node(vi)).

Proposition 2. Suppose there exists an ε-feasible FIPfrom nki,pi to ngoal contained in Ri. Then there exists anε-feasible path in Gi from vi

start to vigoal.

Proof. Suppose there exists an ε-feasible FIP π =(π1, π2, . . . , πL) from π1 = nki,pi = node(vi

start) to πL =ngoal contained in Ri. Note that, for each πj , there existsa unique vertex vj ∈ Gi, such that H(πj) ⊂ H(node(vj))and consider the path (v1, v2, . . . , vL) in Gi. Note thatv1 = vi

start and vL = vigoal. Since the path π is ε-feasible,

it follows that πj is not an ε-obstacle. Furthermore,

πj is a descendant of node(vj) ∈ T , hence by thecontrapositive of Proposition 1, vj is not an ε-obstacle.The path (v1, v2, . . . , vL) is then an ε-feasible path fromvi

start to vigoal in Gi.

nstart

ngoal

π1π2π3

π4

π5

π6 π7 π8 π9

π10

π11

(a)

v6

vigoal

v5

v4

v3v2v1

(b)

Fig. 2: (a) Example environmentW with the current cell,nki,pi , shown in red. Example of a ε-feasible FIP π fromnki,pi to ngoal shown in orange; (b) The graph Gi and theunderlying environment partition. vi

start is drawn withred and orange is used for the path (v1, . . . , vL) in Gi

corresponding to π. Note that all the nodes from π7 tongoal are mapped to the same vertex vi

goal.

IV. Algorithm PropertiesA. Completeness

At iteration i, let a valid extension of the currentpartial path candidate πi

start = (nk0,p0 , nk1,p1 , . . . , nki,pi)be an ε-feasible FIP from nki,pi to ngoal that doesnot have common nodes with πi

start. Note that a validextension is a FIP and hence it is a path consistingonly of leafs of T . The following proposition guaran-tees that any valid extension of πi

start is contained inRi =

⋃vi∈Vi H(node(vi)), where Ri is the region in W

represented by the vertices of Gi. Similarly, we will useRi =

⋃vi∈Vi H(node(vi)) to denote the region repre-

sented by the vertices of Gi. We will also use the notationH(πi

start) =⋃i

j=0 H(nkj ,pj ).

Proposition 3. Let i be the current iteration number.Suppose that, for all j = 0, 1, . . . , i − 1, the MSPP algo-rithm backtracked only if there were no valid extensionsof πj

start. Then, any valid extension of πistart is fully

contained in Ri. Furthermore, if the MSPP algorithmbacktracked at iteration i, there is no valid extension ofπi

start.

Proposition 4. The MSPP algorithm is complete.

Proof. The MSPP algorithm performs an informeddepth-first search on a finite tree Tpath, tree of ε-feasibleloopless FIP starting from nstart, so it visits each branchof the tree Tpath at most once and terminates in finitetime. Suppose now that there exists an ε-feasible FIPπ from nstart to ngoal. Suppose, for the sake of contra-diction, that π is not found by the MSPP algorithm.It follows that either the MSPP algorithm returns adifferent ε-feasible FIP π′ or it backtracks from nstartand reports failure (see Line 15 in Algorithm 1). Suppose

that the MSPP algorithm backtracks from nstart, say,at iteration i. It then follows that πi

start = (nstart).However, every FIP path from nstart has nstart as itsfirst element, and hence πi

start ⊂ π. The last expressionimplies, however, that πi

start can be extended to ngoalusing an ε-feasible FIP, namely, π. From Proposition 3it follows that the algorithm does not backtrack fromnstart, a contradiction. Hence the algorithm returns theε-feasible FIP π′.

B. ComplexityThe reduced graph Gi keeps nodes of size 1 in a sphere

of radius α centered at nki,pi , nodes of size 2 in a sphereof radius 2α centered at nki,pi , etc., and nodes of size 2`

in a sphere of size 2`α centered at nki,pi . All these spherescontain approximately the same number of nodes S, since

S ≈ volume of the sphere of size 2kα

volume of a node at level k ≈ πd/2

Γ( d2 + 1)

αd

is independent of k. Hence, while the search space growsexponentially, as 2`, the number of nodes of the reducedgraph only grows linearly, as `S. Since the number ofnodes per sphere grows exponentially with the dimensiond, it follows that the number of nodes in Gi is linear inthe number of levels of the tree `, and exponential inthe number of dimensions d, that is, |Vi| = O(`2d). Asa comparison, to solve the same problem on a uniformgrid, the graph would have O(2`d) vertices.

1) Finding the reduced graph nodes: The complexityof this step depends on the number of nodes of the treevisited to find all the vertices in the reduced graph. Itcan be easily shown that the worst case complexity ofthis step is O(|Vi|).

2) Finding adjacency: Every pair of vertices is testedfor adjacency, so |Vi|(|Vi|−1)/2 tests are made. Thus thecomplexity of this step is O(|Vi|2).

3) Finding the shortest path: If the A* algorithm isused for this step, the complexity is O(|Ei|+ |Vi| log |Vi|)but |Ei| is bounded by a linear function of |Vi| becausethe number of neighbors of a given hypercube is upperbounded since the resolution is finite. Hence the com-plexity of this step is O(|Vi| log |Vi|).

C. Algorithm Parameter TuningThere are three parameters that can be tuned in the

MSPP algorithm and which affect its performance: themaximum depth of the tree `, the threshold α used tocalculate the reduced graph, and the threshold ε.

1) Maximum number of levels of the tree `: Thisparameter determines at which level of detail the worldmap is used and, subsequently, the resolution of thesmallest nodes of the resulting path.

2) Decomposition parameter α: The path constructedby the algorithm should be a FIP. This is achieved bychoosing only the finest information nodes to be part ofthe path. The following proposition gives a condition onthe parameter α ensuring that the neighbors of nki,pi onGi are leafs of T .

(a) α = 149 nodes

(b) α = 2217 nodes

(c) α = 52377 nodes

Fig. 3: Reduced graph for different values of α. Theoriginal space contains 287,496 nodes.

Proposition 5. Let α ≥√d /2. Then the selected nodes

neighboring the current node are finest information nodes,that is, they are leafs of T .

Figure 3 shows the result of different values for α whenthe point of interest is the corner of a cube (d = 3).

V. Application to a Mobile RobotA. Maps Created From a Vision Sensor

In this section, we describe how we build a 3D multi-resolution volumetric occupancy map of an environ-ment from camera images, similarly to [10]. Given asequence of camera images, we first obtain a camerapath and a sparse 3D reconstruction by performing visualSLAM [15], [14]. We model the 3D environment withthe data structure described in Section II. However,unlike the traditional occupancy mapping work of [21],[19], we do not assume that each voxel’s occupancyonkj,pj

is independent of the other voxels. Instead, weform a 3D conditional random field (CRF) [20] over thevoxel occupancy states {onkj,pj

} that enforces spatialregularization over neighboring voxels as follows

P ({onkj,pj}|D) =

1Z(D)

∏j

ψu(onkj,pj)

∏j,m∈N

ψp(onkj,pj,onkm,pm

), (3)

where Z(D) is the partition function over the observeddata D, and ψu, ψp are unary and pairwise potentials [20]described below. The unary potential ψu(onkj,pj

) is de-fined over each voxel occupancy state onkj,pj

. We usethe sparse point cloud obtained from the visual SLAMpipeline as range measurements to update the unaryterms in the same way as laser range measurementsare treated in traditional occupancy mapping frame-work [19], [21]. In (3) ψp(onkj,pj

, onkm,pm) is the pair-

wise potential, enforcing label consistency between twoneighboring voxels nkj ,pj and nkm,pm falling into a givenneighborhood. We use a Potts potential [20] for ψp, whichtakes a higher value when labels are the same, and hasa lower value when they are different, thus penalizingdissimilar labels across neighboring voxels. The finaloccupancy map is obtained by Maximum a Priori (MAP)inference [9] over the CRF in (3).

Figure 4 shows the results of the map creation and theplanning at the same time applied to the CamVid [2] andLeuven [11] datasets. The Leuven dataset, specifically,

demonstrates some of the potential pitfalls of having ashort fine resolution horizon. In particular, the path isnot smooth exhibiting several zig-zags. This is owing tothe fact that if the vehicle is far from the wall, the wallwill not be resolved as an obstacle until the robot comescloser. This may create a path that keeps zigzaggingnear the wall as the robot tries different alternatives.Nonetheless, the robot eventually finds a path to moveforward.

(a) CamVid (b) Leuven

Fig. 4: Results of the planning on the maps reconstructedfrom the camera images

B. Real-time Application with Unknown Space Explo-ration

In this section, we present another application of theMSPP algorithm in which the robot is equipped with alaser range sensor and has to reach a known goal whilenavigating inside an a priori unknown environment. Weuse a real-time simulation environment which createsstrict constraints on runtime. The map is built from thelaser sensor measurements using Octomap [7], and it usesan incremental method that is fast enough for real timeimplementation. The MSPP algorithm is used to plan onthe partially unknown map, and replanning is done whenobstacles are detected along the current planned path.

We use the Gazebo simulation environment and theRobotic Operating System (ROS) to communicate be-tween the different modules of the simulator. The sim-ulator provides the ground truth representation of theworld and integrates the dynamics of the robot based onthe received commands. Noisy laser measurements aregenerated at 2Hz and the pose of the robot is sent at100Hz. The Octomap server creates the tree T using themeasurements received, and sends the new map to theplanner. The planner checks whether the current plannedpath is ε-feasible. If it is not ε-feasible it replans until itfinds one, and sends the solution to the path tracker as aset of waypoints. Finally, the waypoint tracker generatesthe motor commands from the current robot pose andthe computed waypoints.

The world used for the simulation is shown in Figure 5.The robot starts at the center and the goal is the red ball.The path is recalculated when a new goal is assigned, orthe current planned path is not ε-feasible any more onthe updated map. The computed path is subsequentlysmoothed and is fed to the trajectory tracking module.Figure 6 shows the result of the path smoothing.

Fig. 5: Maze used for the simulation, the red ball rep-resents the goal. The robot starts at the center of themaze.

Figure 6 depicts some key frames of the results1. Thebenefits of using a multi-scale representation can beseen since the unknown space is represented by nodesencoding large regions of the environment, thus reducingthe size of the data kept in memory. The colored cubescorrespond to points measured by the laser sensor, andclearly show the walls.

(a) Initial path (b) First path re-calculation

(c) Second pathrecalculation

(d) Third path re-calculation

(e) Fourth pathrecalculation

(f) Goal reached

Fig. 6: Results of the mapping and planning simulation.

VI. ConclusionsIn this paper, we present a multi-scale perception and

path-planning algorithm. The algorithm is shown to becomplete. The complexity of the algorithm is shown togrow only linearly, while the size of the map grows expo-nentially. This allows the algorithm to be implementedon large maps without excessive execution times. Theproposed algorithm seamlessly integrates multi-scale per-ception with multi-scale path planning. The results of thealgorithm are applied on 2D and 3D maps created froma realistic multi-scale perception algorithm involving amobile robot navigating in an unknown environment.

1A video of the results can be found in the multimedia attach-ments.

References[1] S. Behnke. Local multiresolution path planning. In Robocup

2003: Robot Soccer World Cup VII, pages 332–343. Springer,2004.

[2] G. Brostow, J. Fauqueur, and R. Cipolla. Semantic objectclasses in video: A high-definition ground truth database.Pattern Recognition Letters, 30(2):88–97, 2009.

[3] R. V. Cowlagi. Hierarchical Motion Planning for AutonomousAerial and Terrestrial Vehicles. PhD thesis, Georgia Instituteof Technology - School of Aerospace Engineering, 2011.

[4] D. Ferguson and A. Stentz. Using interpolation to improvepath planning: The Field D* algorithm. Journal of FieldRobotics, 23(2):79–101, 2006.

[5] P. E. Hart, N. J. Nilsson, and B. Raphael. A formal basisfor the heuristic determination of minimum cost paths. IEEETransactions on Systems Science and Cybernetics, 4(2):100–107, 1968.

[6] R. C. Holte, T. Mkadmi, R. M. Zimmer, and A. J. MacDonald.Speeding up problem solving by abstraction: A graph orientedapproach. Artificial Intelligence, 85(1):321–361, 1996.

[7] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, andW. Burgard. OctoMap: An efficient probabilistic 3D mappingframework based on octrees. Autonomous Robots, 34(3):189–206, 2013.

[8] S. Kambhampati and L. S. Davis. Multiresolution pathplanning for mobile robots. IEEE Journal of Robotics andAutomation, 2(3):135–145, 1986.

[9] J. H. Kappes, M. Speth, G. Reinelt, and C. Schnorr. To-wards efficient and exact map-inference for large scale discretecomputer vision problems via combinatorial optimization. InComputer Vision and Pattern Recognition, Portland, OR,USA, June 25–27 2013.

[10] A. Kundu, Y. Li, F. Dellaert, F. Li, and J. M. Rehg. Joint Se-mantic Segmentation and 3D Reconstruction from MonocularVideo. In European Conference on Computer Vision, Zurich,Switzerland, September 6–12, 2014.

[11] L. Ladicky, P. Sturgess, C. Russell, S. Sengupta, Y. Bastanlar,W. Clocksin, and P. H. Torr. Joint optimisation for objectclass segmentation and dense stereo reconstruction. In BritishMachine Vision Conference, Aberystwyth, Wales, August 31– September 3 2010.

[12] J. D. Lafferty, A. McCallum, and F. C. N. Pereira. Conditionalrandom fields: Probabilistic models for segmenting and label-ing sequence data. In International Conference on MachineLearning, Williamstown, MA, USA, June 28 – July 1 2001.

[13] Y. Lu, Y. Huo, and P. Tsiotras. An accelerated path-findingalgorithm using multiscale information. IEEE Transactionson Automatic Control, 57(5):1166–1178, May 2012.

[14] R. Newcombe and A. Davison. Live dense reconstruction witha single moving camera. In Computer Vision and PatternRecognition, San Francisco, CA, USA, June 13–18 2010.

[15] D. Nister, O. Naroditsky, and J. Bergen. Visual odometry. InComputer Vision and Pattern Recognition, Washington, DC,USA, June 27– July 2 2004.

[16] D. K. Pai and L.-M. Reissell. Multiresolution rough terrainmotion planning. IEEE Transactions on Robotics and Au-tomation, 14(1):19–33, 1998.

[17] C. Petres, Y. Pailhas, P. Patron, Y. Petillot, J. Evans, andD. Lane. Path planning for autonomous underwater vehicles.IEEE Transactions on Robotics, 23(2):331–341, 2007.

[18] S. Sahni. Data Structures, Algorithms, and Applications inC++, pages 751–786. New York: WCB McGraw-Hill, 1998.

[19] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics.MIT Press, 2005.

[20] C. Wang, N. Komodakis, and N. Paragios. Markov randomfield modeling, inference and learning in computer vision andimage understanding: A survey. Computer Vision and ImageUnderstanding, 117(11):1610 – 1627, 2013.

[21] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, andW. Burgard. OctoMap: A probabilistic, flexible, and compact3D map representation for robotic systems. In ICRA 2010Workshop on Best Practice in 3D Perception and Modelingfor Mobile Manipulation, Anchorage, Alaska, USA, May 3–82010.

[22] M. Yguel, O. Aycard, and C. Laugier. Wavelet occupancygrids: a method for compact map building. In Field andService Robotics, pages 219–230. Springer, 2006.