Adding option to use penalty for back steps.
Adding option to use penalty for back steps.

Adding functionality to put a penalty on backward steps to promote forward steps in the journey. If no forward steps exist the backward steps will be used.
Stopping Dijkstra if none of the unvisited nodes can be reached.

edgarriba/OpenSfM,onecue/OpenSfM,oscarlorentzon/OpenSfM,sunbingfengPI/OpenSFM_Test,NicWayand/OpenSfM,edgarriba/OpenSfM,oscarlorentzon/OpenSfM,BrookRoberts/OpenSfM,NicWayand/OpenSfM,BrookRoberts/OpenSfM,BrookRoberts/OpenSfM,oscarlorentzon/OpenSfM,sunbingfengPI/OpenSFM_Test,onecue/OpenSfM,BrookRoberts/OpenSfM,onecue/OpenSfM,NicWayand/OpenSfM,sunbingfengPI/OpenSFM_Test,sunbingfengPI/OpenSFM_Test,edgarriba/OpenSfM,mapillary/OpenSfM,edgarriba/OpenSfM,oscarlorentzon/OpenSfM,BrookRoberts/OpenSfM,mapillary/OpenSfM,mapillary/OpenSfM,onecue/OpenSfM,oscarlorentzon/OpenSfM,onecue/OpenSfM,mapillary/OpenSfM,NicWayand/OpenSfM,edgarriba/OpenSfM,sunbingfengPI/OpenSFM_Test,mapillary/OpenSfM,NicWayand/OpenSfM