diff --git a/app/src/main/java/de/apps4ics/mountainnavigation/MainActivity.java b/app/src/main/java/de/apps4ics/mountainnavigation/MainActivity.java
index 89b17c9..6d437d2 100644
--- a/app/src/main/java/de/apps4ics/mountainnavigation/MainActivity.java
+++ b/app/src/main/java/de/apps4ics/mountainnavigation/MainActivity.java
@@ -138,8 +138,7 @@
 
     private List<Marker>[] poiMarkers;
 
-    protected static GeoPoint pathMarkerOne;
-    protected static GeoPoint pathMarkerTwo;
+    protected static ArrayList<OverlayItem> pathMarkers;
 
     private String provider;
     private Location mLocation;
@@ -153,6 +152,7 @@
         setContentView(R.layout.activity_main);
 
         res = getResources();
+        pathMarkers = new ArrayList<>();
 
         cm = (ConnectivityManager) getSystemService(CONNECTIVITY_SERVICE);
         activeNetwork = cm.getActiveNetworkInfo();
@@ -428,6 +428,46 @@
         }
     }
 
+    private int getPos(int type){
+        int pos;
+        switch(type){
+            case Types.FOUNTAIN:
+                pos = 0;
+                break;
+            case Types.PATH:
+                pos = 1;
+                break;
+            case Types.HUT:
+                pos = 2;
+                break;
+            case Types.PEAK:
+                pos = 3;
+                break;
+            case Types.BREAK_POINT:
+                pos = 4;
+                break;
+            case Types.TRASH_BIN:
+                pos = 5;
+                break;
+            case Types.PIC:
+                pos = 6;
+                break;
+            case Types.CELL_RECEPTION:
+                pos = 7;
+                break;
+            case Types.WIFI:
+                pos = 8;
+                break;
+            case Types.LIFT:
+                pos = 9;
+                break;
+            default:
+                pos = 0;
+                break;
+        }
+        return pos;
+    }
+
     private int getType(int position){
         int type;
         switch (position){
@@ -828,11 +868,29 @@
 
         List<OverlayItem> overlayItemList = new ArrayList<>();
         for(Location l : foundLocations){
-            OverlayItem oI = new OverlayItem(null, null, new GeoPoint(l.getLatitude(), l.getLongitude()));
+            OverlayItem oI = new OverlayItem(null, null, new GeoPoint(l.getLatitude(), l.getLongitude(), l.getAltitude()));
             oI.setMarker(res.getDrawable(R.mipmap.ic_poi_red));
             overlayItemList.add(oI);
         }
 
+        List<Poi> poiMarker = dbHandler.getAllPois();
+        for(Poi p : poiMarker){
+            GeoPoint gp = new GeoPoint(p.getLat(), p.getLon(), p.getAlt());
+            OverlayItem oI = new OverlayItem(String.valueOf(p.getId()), String.valueOf(p.getType()), gp);
+            Drawable[] iconLayer = new Drawable[2];
+            iconLayer[0] = res.getDrawable(R.mipmap.ic_poi);
+            BitmapDrawable bd = (BitmapDrawable) res.getDrawable(entryImgs[getPos(p.getType())]);
+            if(bd == null) continue;
+            Bitmap b = bd.getBitmap();
+            Bitmap bResized = Bitmap.createScaledBitmap(b, b.getWidth()/2, b.getHeight()/2, false);
+            bd = new BitmapDrawable(res, bResized);
+            bd.setGravity(Gravity.CENTER_HORIZONTAL);
+            iconLayer[1] = bd;
+            LayerDrawable icon = new LayerDrawable(iconLayer);
+            oI.setMarker(icon);
+            overlayItemList.add(oI);
+        }
+
         AlertDialog.Builder builder = new AlertDialog.Builder(MainActivity.this);
         builder.setView(popupMapView)
                 .setTitle(R.string.path_map_dialog_title)
@@ -902,20 +960,22 @@
                                 }
                             }
                             //TODO add start and end instead of one Location (lat, lon)
-                            if(pathMarkerOne == null || pathMarkerTwo == null){
+                            if(pathMarkers.size() < 2){
                                 Log.e(TAG, "One of the path markers in addPathDialog is null!");
                             }
-                            GeoPoint start = pathMarkerOne;
-                            GeoPoint end = pathMarkerTwo;
 
                             RoadManager roadManager = new OSRMRoadManager();
                             ArrayList<GeoPoint> waypoints = new ArrayList<>();
-                            waypoints.add(start);
-                            waypoints.add(end);
+                            for(int i=0; i<pathMarkers.size(); ++i){
+                                GeoPoint gp = (GeoPoint) pathMarkers.get(i).getPoint();
+                                waypoints.add(gp);
+                                if(pathMarkers.get(i).getTitle() == null) Log.d(TAG, "Waypoint #" + i + " is new!");
+                            }
                             Road road = roadManager.getRoad(waypoints);
                             Polyline line = RoadManager.buildRoadOverlay(road, getApplicationContext());
                             mapView.getOverlays().add(line);
                             mapView.invalidate();
+                            pathMarkers = new ArrayList<>();
 
                             int _type = Types.PATH;
                             long rowId = addPoi(_type);
diff --git a/app/src/main/java/de/apps4ics/mountainnavigation/MapDialogItemizedIconOverlay.java b/app/src/main/java/de/apps4ics/mountainnavigation/MapDialogItemizedIconOverlay.java
index 02f8faf..fd8e788 100644
--- a/app/src/main/java/de/apps4ics/mountainnavigation/MapDialogItemizedIconOverlay.java
+++ b/app/src/main/java/de/apps4ics/mountainnavigation/MapDialogItemizedIconOverlay.java
@@ -6,6 +6,10 @@
 import android.content.res.Resources;
 import android.graphics.drawable.Drawable;
 
+import org.osmdroid.bonuspack.overlays.Polyline;
+import org.osmdroid.bonuspack.routing.OSRMRoadManager;
+import org.osmdroid.bonuspack.routing.Road;
+import org.osmdroid.bonuspack.routing.RoadManager;
 import org.osmdroid.util.GeoPoint;
 import org.osmdroid.views.MapView;
 import org.osmdroid.views.overlay.ItemizedIconOverlay;
@@ -26,6 +30,12 @@
     private Drawable negIcon;
     private MapView mapView;
 
+    private RoadManager roadManager;
+    private ArrayList<GeoPoint> waypoints;
+    private ArrayList<Integer> waypointIds;
+    private Road road;
+    private Polyline line;
+
     public MapDialogItemizedIconOverlay(final Context context, final List<OverlayItem> aList, AlertDialog alertDialog, MapView mapView) {
         super(context, aList, new OnItemGestureListener<OverlayItem>() {
             @Override
@@ -40,6 +50,7 @@
         });
         this.context = context;
         this.items = new ArrayList<>();
+        this.waypointIds = new ArrayList<>();
         this.alertDialog = alertDialog;
         this.mapView = mapView;
         this.selected = 0;
@@ -48,7 +59,12 @@
         negIcon = res.getDrawable(R.mipmap.ic_poi_red);
         for(int i=0; i<aList.size(); ++i){
             items.add(false);
+            waypointIds.add(-1);
         }
+
+        roadManager = new OSRMRoadManager();
+        waypoints = new ArrayList<>();
+        road = new Road();
     }
 
     @Override
@@ -78,6 +94,7 @@
     public boolean addItem(OverlayItem item) {
         boolean ret = super.addItem(item);
         items.add(false);
+        waypointIds.add(-1);
         return ret;
     }
 
@@ -86,6 +103,10 @@
         OverlayItem oI = super.removeItem(position);
         if(items.get(position)) selected--;
         items.remove(position);
+        int waypointIndex = waypointIds.get(position);
+        waypoints.remove(waypointIndex);
+        waypointIds.set(position, -1);
+        MainActivity.pathMarkers.remove(waypointIndex);
         checkForSelection();
         return oI;
     }
@@ -95,32 +116,32 @@
             items.set(index, false);
             selected--;
             getItem(index).setMarker(negIcon);
+            int waypointIndex = waypointIds.get(index);
+            waypoints.remove(waypointIndex);
+            waypointIds.set(index, -1);
+            MainActivity.pathMarkers.remove(waypointIndex);
         } else {
             items.set(index, true);
             selected++;
             getItem(index).setMarker(posIcon);
+            waypoints.add((GeoPoint) getItem(index).getPoint());
+            waypointIds.set(index, waypoints.size()-1);
+            MainActivity.pathMarkers.add(getItem(index));
         }
         mapView.invalidate();
         checkForSelection();
     }
 
     private void checkForSelection(){
-        if(selected == 2){
-            boolean firstFound = false;
-            for(int i=0; i<items.size(); ++i){
-                if(items.get(i)){
-                    if(!firstFound){
-                        MainActivity.pathMarkerOne = (GeoPoint) getItem(i).getPoint();
-                        firstFound = true;
-                    } else {
-                        MainActivity.pathMarkerTwo = (GeoPoint) getItem(i).getPoint();
-                    }
-                }
-            }
+        if(selected >= 2){
+            mapView.getOverlays().remove(line);
+            road = roadManager.getRoad(waypoints);
+            line = RoadManager.buildRoadOverlay(road, context);
+            mapView.getOverlays().add(line);
+            mapView.invalidate();
             alertDialog.getButton(AlertDialog.BUTTON_POSITIVE).setEnabled(true);
         } else {
-            MainActivity.pathMarkerOne = null;
-            MainActivity.pathMarkerTwo = null;
+            mapView.getOverlays().remove(line);
             alertDialog.getButton(AlertDialog.BUTTON_POSITIVE).setEnabled(false);
         }
     }