save incremental work
save incremental work
replanning
save incremental work
so far
type refactoring didn't work
Merge branch 'bsp' into merge

Conflicts:
	src/bsp/belief_func.hpp
	src/bsp/bsp_problem_helper.hpp
	src/bsp/common.hpp
	src/bsp/optimizers.cpp
	src/bsp_sandbox/car.cpp
	src/bsp_sandbox/toy.cpp
	src/bsp_sandbox/toy.hpp
integrate outer alpha loop
make all 3 examples running