Changed dp_slam interface, bugfix or segfault in gridmap