Open molinadavid opened 5 years ago
Hi David,
Thanks for sharing the screen recordings and the database. I could try re-run the mapping with different parameters.
Modifying the grid map only works when memory management is disabled and in localization mode:
Rtabmap/TimeThr=0
)Mem/IncrementalMemory=false
), it should use the manually modified grid map and not update it afterwardsI noticed that processing time for image registrations (see "Neighbor link refining" and "Add loop closure link" timings below) is quite high on the robot. RGBD/LoopClosureReextractFeatures
is not necessary in the new versions as raw descriptors are saved by default in nodes, you will save processing time. As the ground is a plane, you could disable normals segmentation Grid/NormalsSegmentation
to save >50% time on creating the local grids. As RGBD/OptimizeMaxError
is set, Optimizer/Robust
should be false.
RGBD/LoopClosureReextractFeatures=false
Grid/NormalsSegmentation=false
Optimizer/Robust=false
If you want to re-use the same database you created with those new parameters, you can regenerate the map like that (make sure you have latest rtabmap version from source to do this):
$ rtabmap-reprocess --Rtabmap/TimeThr 0 --RGBD/LoopClosureReextractFeatures false --Grid/NormalsSegmentation false --Optimizer/Robust false rtabmap_1turn_optimized.db output.db
I also noticed that as RGBD/NeighborLinkRefining=true
, the resulting refined trajectory looks strange when the robot rotates on place ( while the resulting assembled cloud looks okay):
It creates always an "arc"-like correction. Normally, if TF between /base_link and /camera_link is perfect, the correction should not create a systematic arc like that. The error is consistent with all rotations in the map, so the problem would be that static TF is not accurate enough. Note that if the camera TF is fixed and that odometry doesn't drift too much, you could disable
RGBD/NeighborLinkRefining
to save some processing time. Make sure to set realistic covariance for your input odometry in rtabmap node:
<param name="odom_frame_id" value="odom"/>
<param name="odom_tf_linear_variance" value="0.001"/>
<param name="odom_tf_angular_variance" value="0.005"/>
For the jumps during navigation, the local planner could be less constraint to follow exactly the path, to move less in zigzag on relocalization. If you could have a second database, we would be able to replay it against the first map in localization mode to better tune the parameters to decrease the jumps.
cheers, Mathieu
Hi Mathieu,
Thank you very much for your response, I am currently testing all the ideas you shared and will inform as soon as I finish, I will also record a second bag later today to create a different database and test the navigation as you suggested and will share it later when is done.
Regarding the "arc-like" turn in place you mentioned, could it be explained by the fact that the camera is actually on the front of the robot and the differential wheels are on the back of the body?, so from the point of view of the camera it will always create an arc, or do I need to better verify the tf description?
Hi Mathieu,
I made another database for you to compare as requested, if you want anything else (like the rosbag used for this map) please let me know.
I made a mistake on the previous database, this is the correct one. rtabmap.db
Best regards, David
Hi David,
Thx for the second database. It helped a lot to see why localization jumps often. The cause is mainly that the environment can change visually quite a lot in the range of the depth sensor (e.g. objects having good depth estimation are often those who are not there in the second traversal). The ceiling has no depth estimation most of the time, which limits the 3d features to boxes that are not static. We can reduce some big jumps by forcing a higher minimum of matched visual features (e.g., Vis/MinInliers
=20 instead of 5), at the cost of accepting localization less often. Here an example where only features on the far left side of the image can be used for localization.
Here are some different configurations I tested. I called the mapping database mapping.db and the localization/navigation one localization.db.
$ rtabmap-reprocess --Rtabmap/TimeThr 0 --RGBD/LoopClosureReextractFeatures false --Grid/NormalsSegmentation false --Optimizer/Robust false --Optimizer/Strategy 2 --Vis/MinInliers 20 mapping.db mapping_reprocessed.db
Then running in localization mode (with localization.db as input source) rtabmap with this reprocessed database:
$ rtabmap mapping_reprocessed.db
The white line is the trajectory during localization. I've found that at some places, localization is not possible as features are only taken on foreground where depth is valid:
Note how the depth image at the bottom overlaps only where objects have changed.
Added parameters: --Mem/DepthAsMask false --Vis/DepthAsMask false
$ rtabmap-reprocess --Rtabmap/TimeThr 0 --RGBD/LoopClosureReextractFeatures false --Grid/NormalsSegmentation false --Optimizer/Robust false --Optimizer/Strategy 2 --Vis/MinInliers 20 --Mem/DepthAsMask false --Vis/DepthAsMask false mapping.db mapping_reprocessed.db
Then running in localization mode (with localization.db as input source) rtabmap with this reprocessed database:
$ rtabmap mapping_reprocessed.db
Loop closures are found more often, though not necessary accurate when not enough features with valid depth can be extracted. Compare that image with the one of the first configuration, now far features in the center of images (where there is no depth) can be used for localization.
Added parameters: --RGBD/LocalBundleOnLoopClosure true
(latest rtabmap version from source should be used to work properly in 2D when Reg/Force3DoF
is true)
$ rtabmap-reprocess --Rtabmap/TimeThr 0 --RGBD/LoopClosureReextractFeatures false --Grid/NormalsSegmentation false --Optimizer/Robust false --Optimizer/Strategy 2 --Vis/MinInliers 20 --Mem/DepthAsMask false --Vis/DepthAsMask false --RGBD/LocalBundleOnLoopClosure true --RGBD/ mapping.db mapping_reprocessed.db
Then running in localization mode (with localization.db as input source) rtabmap with this reprocessed database:
$ rtabmap mapping_reprocessed.db
The localizations are a little more smoother.
I am not sure we can do a lot better in this dynamic environment with VSLAM. A complement solution for localization in areas where the environment changed quite drastically could be to use ArUco markers or apriltags on the ceiling (with another camera looking up). There is an example here with rtabmap + apriltags2.
EDIT For the "arc-like": trajectories, this is a static TF problem. If TF between wheel odometry frame (e.g., base_link which would be the middle shaft of the wheels) and camera frame is accurate, there would be no arc.
Hi Mathieu,
Again, thank you very much for all your help and time, I will test all your suggestions and will report soon the results.
Best regards, David
Hi Mathieu,
Just a quick question, could you tell me the settings of your environment? I am having problems running the rtabmap-reprocess with a infinite jac error (I suspect from g2o).
Then trying to use a different version of g2o I get the following error on the make stage
rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:497:26: error: ‘priorEdge’ was not declared in this scope
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
Here is my current setup for rtabmap (output of cmake, I build it from source):
-- --------------------------------------------
-- Info :
-- Version : 0.19.0
-- CMAKE_INSTALL_PREFIX = /home/mathieu/catkin_ws/devel
-- CMAKE_BUILD_TYPE = Release
-- CMAKE_INSTALL_LIBDIR = lib
-- BUILD_APP = ON
-- BUILD_TOOLS = ON
-- BUILD_EXAMPLES = ON
-- BUILD_SHARED_LIBS = ON
-- CMAKE_CXX_FLAGS = -fmessage-length=0 -fopenmp -std=c++11
-- FLANN_KDTREE_MEM_OPT = OFF
-- PCL_DEFINITIONS = -DEIGEN_USE_NEW_STDVECTOR;-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET;-DFLANN_STATIC;-Dqh_QHpointer
-- Optional dependencies ('*' affects some default parameters) :
-- *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)
-- With external SQLite3 = YES (License: Public Domain)
-- With Freenect = YES (License: Apache v2 and/or GPLv2)
-- With OpenNI2 = YES (License: Apache v2)
-- With Freenect2 = YES (License: Apache v2 and/or GPLv2)
-- With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)
-- With dc1394 = YES (License: LGPL)
-- With FlyCapture2/Triclops = NO (Point Grey SDK not found)
-- With TORO = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])
-- *With g2o = YES (License: BSD)
-- *With GTSAM = YES (License: BSD)
-- With VERTIGO = YES (License: GPLv3)
-- With cvsba = NO (cvsba not found)
-- *With libpointmatcher = YES (License: BSD)
-- With loam_velodyne = NO (loam_velodyne not found)
-- With ZED = NO (WITH_ZED=OFF)
-- With RealSense = YES (License: Apache-2)
-- With RealSenseSlam = NO (WITH_REALSENSE_SLAM=OFF)
-- With RealSense2 = YES (License: Apache-2)
-- With OCTOMAP = YES (License: BSD)
-- With CPUTSDF = NO (CPUTSDF not found)
-- With OpenChisel = NO (open_chisel not found)
-- With libfovis = NO (libfovis not found)
-- With libviso2 = NO (libviso2 not found)
-- With dvo_core = NO (dvo_core not found)
-- With okvis = NO (okvis not found)
-- With msckf_vio = NO (WITH_MSCKF_VIO=OFF)
-- With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)
-- With Qt5 = YES (License: Open Source or Commercial)
-- --------------------------------------------
Note that it is built with GTSAM and I used it in my commands above (Optimizer/Strategy=2). My system is on Ubuntu 16.04/ROS Kinetic.
Which g2o version are you using? However, this error is strange because EdgeSE3XYZPrior is defined in this header included in rtabmap repo, which should be included in OptimizerG2O.cpp here.
Hi Mathieu,
This is the information of my build, I also build from source.
-- --------------------------------------------
-- Info :
-- Version : 0.18.1
-- CMAKE_INSTALL_PREFIX = /usr/local
-- CMAKE_BUILD_TYPE = Release
-- CMAKE_INSTALL_LIBDIR = lib
-- BUILD_APP = ON
-- BUILD_TOOLS = ON
-- BUILD_EXAMPLES = ON
-- BUILD_SHARED_LIBS = ON
-- CMAKE_CXX_FLAGS = -fmessage-length=0 -fopenmp -std=c++11
-- PCL_DEFINITIONS = -DEIGEN_USE_NEW_STDVECTOR;-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET;-DFLANN_STATIC;-Dqh_QHpointer
-- Optional dependencies ('*' affects some default parameters) :
-- *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)
-- With external SQLite3 = YES (License: Public Domain)
-- With Freenect = YES (License: Apache v2 and/or GPLv2)
-- With OpenNI2 = YES (License: Apache v2)
-- With Freenect2 = NO (libfreenect2 not found)
-- With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)
-- With dc1394 = NO (dc1394 not found)
-- With FlyCapture2/Triclops = NO (Point Grey SDK not found)
-- With TORO = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])
-- *With g2o = YES (License: BSD)
-- *With GTSAM = YES (License: BSD)
-- With VERTIGO = YES (License: GPLv3)
-- With cvsba = YES (License: GPLv2)
-- *With libpointmatcher = NO (libpointmatcher not found)
-- With loam_velodyne = NO (loam_velodyne not found)
-- With ZED = YES (With CUDA)
-- With RealSense = YES (License: Apache-2)
-- With RealSenseSlam = NO (WITH_REALSENSE_SLAM=OFF)
-- With RealSense2 = NO (librealsense2 not found)
-- With OCTOMAP = YES (License: BSD)
-- With CPUTSDF = NO (CPUTSDF not found)
-- With OpenChisel = NO (open_chisel not found)
-- With libfovis = NO (libfovis not found)
-- With libviso2 = NO (libviso2 not found)
-- With dvo_core = NO (dvo_core not found)
-- With okvis = NO (okvis not found)
-- With msckf_vio = NO (WITH_MSCKF_VIO=OFF)
-- With ORB_SLAM2 = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)
-- With Qt5 = YES (License: Open Source or Commercial)
-- --------------------------------------------
I built gtsam 3.1.0 from source, my system is Ubuntu 16.04/ROS Kinetic.
My g2o is also built from source, the error I reported before I got it trying to re make rtabmap using different version of g2o (from different source and from apt) but I can't remember the exact combination at the moment.
However I found that the infinite jac error happens when I try to re-process databases made from a ros bag that was created on a different computer than mine. The process to create this databases is as follow.
The hardware is as follow
As you can see the only differences between the computers are the Cuda version, I don't know if that can be the trigger for the error but the bags that are recorded on my pc (Cuda 9) have no problem to re-process, while the bags recorded on the other pc (Cuda 10) always fail the re-process with infinite jac error.
Running your command on a database created from the bag on the other pc gives the following output
Custom parameters:
Grid/NormalsSegmentation = false
Mem/DepthAsMask = false
Optimizer/Robust = false
Optimizer/Strategy = 2
RGBD/LocalBundleOnLoopClosure = true
RGBD/LoopClosureReextractFeatures = false
Rtabmap/TimeThr = 0
Vis/DepthAsMask = false
Vis/MinInliers = 20
Set working directory to ".".
Reprocessing data of "rtabmap_surf_v2.db"...
High variance detected, triggering a new map...
[ INFO:0] Initialize OpenCL runtime...
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
X server found. dri2 connection failed!
DRM_IOCTL_I915_GEM_APERTURE failed: Invalid argument
Assuming 131072kB available aperture size.
May lead to reduced performance or incorrect rendering.
get chip id failed: -1 [22]
param: 4, val: 0
Processed 1/935 nodes [0]... 754ms
[SetJac] infinite jac
Signal 6 caught...
Aborted (core dumped)
The database used forthis can be found here
Again I can not thank you enough for all your help (and the project).
Best regards, David
You may try launching rtabmap-reprocess
with gdb:
$ gdb --args rtabmap-reprocess --udebug rtabmap_surf_v2.db
Type run
, then after it crashes type backtrace
, copy/paste the full output here. It crashes after the first node processed. It could be an Eigen incompatibility error.
You should use GTSAM >=4. Make sure GTSAM is built with GTSAM_USE_SYSTEM_EIGEN cmake option to ON.
For g2o, make sure it is built with BUILD_WITH_MARCH_NATIVE cmake option to OFF.
Hi Mathieu,
Thank you very much for getting back so quick, I will try to add as much details as possible here about the different issues related to this case.
First I have tried building rtabmap again and got the same error explained before, this is the output of the failure message
[ 25%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/OptimizerTORO.cpp.o
[ 26%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/OptimizerG2O.cpp.o
/home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp: In member function ‘virtual std::map<int, rtabmap::Transform> rtabmap::OptimizerG2O::optimize(int, const std::map<int, rtabmap::Transform>&, const std::multimap<int, rtabmap::Link>&, cv::Mat&, std::__cxx11::list<std::map<int, rtabmap::Transform> >*, double*, int*)’:
/home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:497:8: error: ‘EdgeSE3XYZPrior’ was not declared in this scope
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
^
/home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:497:8: note: suggested alternative:
In file included from /usr/local/include/g2o/types/slam3d/types_slam3d.h:50:0,
from /home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:61:
/usr/local/include/g2o/types/slam3d/edge_se3_xyzprior.h:39:30: note: ‘g2o::EdgeSE3XYZPrior’
class G2O_TYPES_SLAM3D_API EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3, VertexSE3>
^
/home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:497:26: error: ‘priorEdge’ was not declared in this scope
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
^
/home/david/rtabmap/corelib/src/optimizer/OptimizerG2O.cpp:497:42: error: expected type-specifier before ‘EdgeSE3XYZPrior’
EdgeSE3XYZPrior * priorEdge = new EdgeSE3XYZPrior();
^
corelib/src/CMakeFiles/rtabmap_core.dir/build.make:769: recipe for target 'corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/OptimizerG2O.cpp.o' failed
make[2]: *** [corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/OptimizerG2O.cpp.o] Error 1
CMakeFiles/Makefile2:272: recipe for target 'corelib/src/CMakeFiles/rtabmap_core.dir/all' failed
make[1]: *** [corelib/src/CMakeFiles/rtabmap_core.dir/all] Error 2
Makefile:151: recipe for target 'all' failed
make: *** [all] Error 2
As for the backtrace log, this is the full output of it.
As always thank you very much for your help, David
This g2o error is strange (same as #363), because that EdgeSE3XYZPrior is defined here. It is maybe the order of the included files. I'll try to build g2o from source and see if I can reproduce this problem. EDIT: It is fixed in this commit.
For the crash log, it crashes in g2o library:
0x00007fffee55f45f in g2o::EdgeProjectP2SC::linearizeOplus() ()
from /usr/local/lib/libg2o_types_sba.so
#3 0x00007ffff79355d9 in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::buildSystem() () from /usr/local/lib/librtabmap_core.so.0.18
#4 0x00007fffeecc21f6 in g2o::OptimizationAlgorithmGaussNewton::solve(int, bool) () from /usr/local/lib/libg2o_core.so
#5 0x00007fffeecba297 in g2o::SparseOptimizer::optimize(int, bool) ()
Can you show the full output of cmake for rtabmap build? This is maybe related to BUILD_WITH_MARCH_NATIVE, in particular is PCL built from source too? The misusage of compilation flag -march=native
across different linked libraries can cause this kind of problem. Or there is a problem with latest version of g2o from source...
Hi Mathieu,
I pulled the latest version of rtabmap including the fix above and I don't get that error anymore, however when I run the make command I am now getting an error related to Boost::timer, the full output is bellow
[ 38%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/clams/slam_calibrator.cpp.o
[ 38%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/opencv/solvepnp.cpp.o
[ 38%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/toro3d/posegraph3.cpp.o
[ 39%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/toro3d/treeoptimizer3_iteration.cpp.o
[ 39%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/toro3d/treeoptimizer3.cpp.o
[ 39%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/toro3d/posegraph2.cpp.o
[ 40%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/toro3d/treeoptimizer2.cpp.o
[ 40%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/g2o/edge_se3_xyzprior.cpp.o
[ 40%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/vertigo/g2o/edge_se2Switchable.cpp.o
[ 41%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/vertigo/g2o/edge_se3Switchable.cpp.o
[ 41%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/vertigo/g2o/edge_switchPrior.cpp.o
[ 41%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/vertigo/g2o/types_g2o_robust.cpp.o
[ 42%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/optimizer/vertigo/g2o/vertex_switchLinear.cpp.o
[ 42%] Building CXX object corelib/src/CMakeFiles/rtabmap_core.dir/OctoMap.cpp.o
[ 42%] Linking CXX shared library ../../../bin/librtabmap_core.so
/usr/bin/ld: cannot find -lBoost::timer
collect2: error: ld returned 1 exit status
corelib/src/CMakeFiles/rtabmap_core.dir/build.make:1975: recipe for target '../bin/librtabmap_core.so.0.19.0' failed
make[2]: *** [../bin/librtabmap_core.so.0.19.0] Error 1
CMakeFiles/Makefile2:272: recipe for target 'corelib/src/CMakeFiles/rtabmap_core.dir/all' failed
make[1]: *** [corelib/src/CMakeFiles/rtabmap_core.dir/all] Error 2
Makefile:151: recipe for target 'all' failed
make: *** [all] Error 2
Also this is the full cmake output cmake.log
See this post for -lBoost::timer
error: https://github.com/introlab/rtabmap_ros/issues/291
Greetings,
I have created a map using rtabmap_ros and processed the database using rtabmap-databaseviewer, after that when I want to navigate that map I find 2 big problems and I would very much appreciate your help solving them.
The first problem I have is that the correction of the robot's pose some times is very big making the robot "jump" on the map long distances, I would like to know how could I minimize this "jumps" to have the robot navigate smoother.
The second problem is actually related to the grid map, as I mentioned on the title I would like to use the globally optimized grid map for navigation, the problem I am facing is that the optimized grid does not remain constant and get "unloaded" and "reconstructed" again some times, but it does not load the full grid globally optimized.
I tried exporting the grid, cleaning it and then re importing it again using the databaseviewer, it works when it first load the database, but then it gets unloaded and the same as before happens.
I also tried publishing the static grid map using map_server, it works for most of the time but there are some areas where the sync is not perfect and there is a risk of the robot thinking that is navigating on a wall or something like that.
in order to help illustrating my issues I will share some screen recordings and the database used for this navigation.
Video showing rtabmap grid unloaded Video showing static map Database used for navigation
Thank you very much in advance for the help you can provide on this.
Best regards, David