Error after changing from binaries to git source
Locked
 
|
This post was updated on .
Hi, rtabmap_ros worked fine for me on Ubuntu 14.04 ROS Indigo since i changed from ubuntu binaries to a source build yesterday. It seems like getting the following error on first loop closure detection:
rtabmap: /build/buildd/pcl-1.7-1.7.1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:172: int pcl::KdTreeFLANN<PointT, Dist>::radiusSearch(const PointT&, double, std::vector<int>&, std::vector<float>&, unsigned int) const [with PointT = pcl::PointXYZ; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"' failed. Thanks for help! |
Administrator
|
This method is used mainly with poses from the graph. The poses should not be null (normally). I added new assert in the rtabmap code so we could get a more precise error. Can you update to latest code and try again?
$ cd rtabmap $ git pull origin master $ cd build $ make install |
|
|
This is the output:
[FATAL] (2015-03-31 17:02:56.217) Graph.cpp:1262::getNodesInRadius() Condition (pcl::isFinite((*cloud)[oi])) not met! [Invalid pose (8) xyz=-nan,-nan,-nan rpy=0.000000,-0.000000,0.000000] ******* FATAL message occurred! Application will now exit. ******* rtabmap: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed. |
|
Administrator
|
Thank you, it seems that the optimized poses are wrong. Are you starting rtabmap with a previous database? I added more asserts, can you try again? This time, you can add "--udebug" argument to rtabmap node and send me the last ~100 log lines.
|
|
|
I have to thank you. No, im starting rtabmap with "--delete_db_on_start" argument every time. Here are the last log lines:
[ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:880::process() Processing signature 50 [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:882::process() timeMemoryUpdate=0.138159s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:957::process() timeScanMatching=0.000025s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1030::process() timeLocalTimeDetection=0.000020s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1047::process() computing likelihood... [DEBUG] (2015-03-31 17:32:02.249) Memory.cpp:1181::computeLikelihood() processing... [DEBUG] (2015-03-31 17:32:02.249) Memory.cpp:1216::computeLikelihood() compute likelihood (tf-idf) 0.000115 s [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:2273::adjustLikelihood() likelihood.size()=12 [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:2292::adjustLikelihood() values.size=9 [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:2339::adjustLikelihood() mean=0.057923, stdDev=0.101316, max=0.321669, maxId=35, time=0.000007s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1056::process() timeLikelihoodCalculation=0.000221s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1062::process() getting posterior... [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:132::computePosterior() [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:411::updatePrediction() [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:439::updatePrediction() time creating id-index maps = 0.000015s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:451::updatePrediction() time getting removed ids = 0.000008s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:491::updatePrediction() time getting ids to update = 0.000040s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:503::updatePrediction() time updating modified/added ids = 0.000127s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:527::updatePrediction() time copying = 0.000008s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:546::updatePrediction() time updating virtual place = 0.000006s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:548::updatePrediction() Modified=10, Added=1, Copied=0 [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:164::computePosterior() STEP1-generate prior=0.000223s, rows=12, cols=12 [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:554::updatePosterior() [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:176::computePosterior() STEP1-update posterior=0.000000s, posterior=12, _posterior size=12 [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:182::computePosterior() STEP1-matrix mult time=0.000036s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:185::computePosterior() STEP1-matrix mult time=0.000007s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:204::computePosterior() STEP2-likelihood time=0.000008s [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:208::computePosterior() sum=1.525715 [DEBUG] (2015-03-31 17:32:02.249) BayesFilter.cpp:216::computePosterior() normalize time=0.000013s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1067::process() timePosteriorCalculation=0.000315s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1079::process() creating hypotheses... [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1093::process() Highest hypothesis=35, value=0.132348, timeHypothesesCreation=0.000014s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1128::process() timeHypothesesValidation=0.000007s [DEBUG] (2015-03-31 17:32:02.249) Memory.cpp:1331::joinTrashThread() [DEBUG] (2015-03-31 17:32:02.249) Memory.cpp:1333::joinTrashThread() [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1154::process() Time emptying memory trash = 0.000035s, joining (actual overhead) = 0.000017s [ INFO] (2015-03-31 17:32:02.249) Rtabmap.cpp:1166::process() Retrieving locations... around id=35 [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:1170::process() margin=17 maxRetieved=2 [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:1182::process() In TIME [DEBUG] (2015-03-31 17:32:02.249) Rtabmap.cpp:1189::process() neighbors of 35 in time = 21 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=35 m=0 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=34 m=1 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=36 m=1 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=33 m=2 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=37 m=2 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=32 m=3 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=38 m=3 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=31 m=4 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=39 m=4 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=1 m=5 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1217::process() nt=40 m=5 immunized=1 [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1233::process() In SPACE [DEBUG] (2015-03-31 17:32:02.250) Rtabmap.cpp:1240::process() neighbors of 35 in space = 21 [ INFO] (2015-03-31 17:32:02.250) Rtabmap.cpp:1290::process() margin=17, neighborhoodSize=17, reactivatedIds.size=11, nbLoadedFromDb=0, nbDirectNeighborsInDb=0, time=0.000117s (0.000000s 0.000000s) [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 49: 0.002626 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 48: 0.004794 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 47: 0.006822 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 46: 0.008366 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 45: 0.009052 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 44: 0.009238 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 43: 0.009550 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 42: 0.011829 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 41: 0.014577 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 40: 0.017069 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 39: 0.019149 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 38: 0.020522 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 37: 0.020817 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 36: 0.020997 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 35: 0.022503 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 34: 0.024556 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 33: 0.026612 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 32: 0.028783 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 31: 0.031787 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1292::getNodesInRadius() Inlier 1: 0.033139 [DEBUG] (2015-03-31 17:32:02.250) Graph.cpp:1297::getNodesInRadius() found nodes=20 [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3888::reactivateSignatures() [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3904::reactivateSignatures() idsToLoad = 0 [DEBUG] (2015-03-31 17:32:02.250) DBDriver.cpp:303::loadSignatures() [DEBUG] (2015-03-31 17:32:02.250) DBDriver.cpp:343::loadSignatures() [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3780::enableWordsRef() size=0 [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3807::enableWordsRef() oldWordIds.size()=0, getOldIds time=0.000000s [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3816::enableWordsRef() loading words(0) time=0.000007s [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3879::enableWordsRef() 0 words total ref added from 0 signatures, time=0.000006s... [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:3920::reactivateSignatures() time = 0.000045s [ INFO] (2015-03-31 17:32:02.250) Rtabmap.cpp:1390::process() retrieval of 0 (db time = 0.000018s) [ INFO] (2015-03-31 17:32:02.250) Rtabmap.cpp:1393::process() total timeRetrievalDbAccess=0.000018s [ INFO] (2015-03-31 17:32:02.250) Rtabmap.cpp:1399::process() timeReactivations=0.000464s [DEBUG] (2015-03-31 17:32:02.250) Memory.cpp:1854::computeVisualTransform() Correspondences = 24 [DEBUG] (2015-03-31 17:32:02.250) util3d.cpp:1322::transformFromXYZCorrespondences() iterations=100 inlierThreshold=0.050000 [DEBUG] (2015-03-31 17:32:02.266) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 21/21, with an error threshold of 0.050000. [DEBUG] (2015-03-31 17:32:02.267) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.035618 (variance=0.000141) on iteration 0 out of 10. [DEBUG] (2015-03-31 17:32:02.267) util3d.cpp:1456::transformFromXYZCorrespondences() RANSAC inliers=21/24 tf=xyz=0.015798,0.012297,-0.006471 rpy=0.001640,-0.008575,0.055060 [DEBUG] (2015-03-31 17:32:02.269) Memory.cpp:2014::computeIcpTransform() Guess transform = xyz=0.015798,0.012297,-0.006471 rpy=0.001640,-0.008575,0.055060 [DEBUG] (2015-03-31 17:32:02.269) Memory.cpp:2135::computeIcpTransform() 2D ICP [ INFO] (2015-03-31 17:32:02.269) Memory.cpp:2143::computeIcpTransform() 2D ICP: Dropping z (-0.006471), roll (0.001640) and pitch (-0.008575) rotation! [DEBUG] (2015-03-31 17:32:02.328) Memory.cpp:2181::computeIcpTransform() hasConverged=true, variance=0.000014, correspondences=504/545 (91.803276%) [DEBUG] (2015-03-31 17:32:02.328) Memory.cpp:2219::computeIcpTransform() New transform = xyz=-0.020599,0.001720,-0.000000 rpy=-0.000000,0.000000,-0.060913 [ INFO] (2015-03-31 17:32:02.328) Memory.cpp:2355::addLink() old=35, new=50 transform: xyz=-0.020599,0.001720,-0.000000 rpy=-0.000000,0.000000,-0.060913 [DEBUG] (2015-03-31 17:32:02.328) Memory.cpp:2367::addLink() Add link between 35 and 50 [DEBUG] (2015-03-31 17:32:02.328) Signature.cpp:130::addLink() Add link 50 to 35 (type=1) [DEBUG] (2015-03-31 17:32:02.328) Signature.cpp:130::addLink() Add link 35 to 50 (type=1) [ INFO] (2015-03-31 17:32:02.328) Rtabmap.cpp:1519::process() timeAddLoopClosureLink=0.078071s [ INFO] (2015-03-31 17:32:02.328) Rtabmap.cpp:1579::process() timeLocalSpaceDetection=0.000012s [ INFO] (2015-03-31 17:32:02.328) Rtabmap.cpp:1593::process() Update map correction: SLAM mode [DEBUG] (2015-03-31 17:32:02.328) Rtabmap.cpp:2229::optimizeCurrentMap() Optimize map: around location 50 [DEBUG] (2015-03-31 17:32:02.328) Rtabmap.cpp:2234::optimizeCurrentMap() get ids=21 [ INFO] (2015-03-31 17:32:02.328) Rtabmap.cpp:2239::optimizeCurrentMap() get ids time 0.000048 s [DEBUG] (2015-03-31 17:32:02.328) Memory.cpp:3930::getMetricConstraints() [ INFO] (2015-03-31 17:32:02.328) Rtabmap.cpp:2244::optimizeCurrentMap() get constraints (21 poses, 21 edges) time 0.000123 s [DEBUG] (2015-03-31 17:32:02.328) Graph.cpp:219::optimize() Optimizing graph (pose=21 constraints=21)... [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:228::optimize() fill poses to TORO... [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:254::optimize() fill edges to TORO... [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:331::optimize() buildMST... [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:348::optimize() Building TORO tree... (if a crash happens just after this msg, TORO is not able to find the root of the graph!) [ INFO] (2015-03-31 17:32:02.330) Graph.cpp:352::optimize() TORO iterate begin (iterations=100) [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:401::optimize() i 0 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:401::optimize() i 1 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:401::optimize() i 2 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:401::optimize() i 3 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.330) Graph.cpp:401::optimize() i 4 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 5 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 6 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 7 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 8 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 9 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 10 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 11 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 12 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 13 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 14 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 15 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 16 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 17 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 18 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 19 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 20 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 21 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 22 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 23 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 24 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 25 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 26 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 27 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 28 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 29 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 30 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 31 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 32 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 33 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 34 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 35 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 36 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 37 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 38 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.331) Graph.cpp:401::optimize() i 39 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 40 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 41 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 42 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 43 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 44 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 45 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 46 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 47 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 48 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 49 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 50 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 51 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 52 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 53 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 54 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 55 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 56 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 57 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 58 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 59 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 60 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 61 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 62 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 63 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 64 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 65 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 66 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 67 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 68 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 69 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 70 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 71 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 72 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 73 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 74 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.332) Graph.cpp:401::optimize() i 75 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 76 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 77 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 78 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 79 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 80 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 81 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 82 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 83 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 84 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 85 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 86 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 87 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 88 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 89 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 90 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 91 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 92 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 93 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 94 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 95 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 96 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 97 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 98 RotGain=1.000000 global error=nan error/constraint=nan [DEBUG] (2015-03-31 17:32:02.333) Graph.cpp:401::optimize() i 99 RotGain=1.000000 global error=nan error/constraint=nan [ INFO] (2015-03-31 17:32:02.333) Graph.cpp:404::optimize() TORO iterate end [FATAL] (2015-03-31 17:32:02.333) Graph.cpp:427::optimize() Condition (!newPose.isNull()) not met! [Optimized pose 31 is null!?!?] ******* FATAL message occurred! Application will now exit. ******* rtabmap: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed. [rtabmap/rtabmap-25] process has died [pid 2710, exit code -6, cmd /home/mark/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start --udebug odom:=/odom scan:=/kinect_scan rgb/image:=/camera/data_throttled_image depth/image:=/camera/data_throttled_image_depth rgb/camera_info:=/camera/data_throttled_camera_info __name:=rtabmap __log:=/home/mark/.ros/log/f0220a9c-d7ba-11e4-9b16-94de8068714b/rtabmap-rtabmap-25.log]. log file: /home/mark/.ros/log/f0220a9c-d7ba-11e4-9b16-94de8068714b/rtabmap-rtabmap-25*.log |
|
Administrator
|
Yeah something is wrong with TORO. Since you are doing 2D SLAM (with ICP 2D), can you try 2D TORO if it fails too? Just add this new parameter under rtabmap node:
<param name="RGBD/OptimizeSlam2D" type="string" value="true"/> |
|
|
It seems that the same thing is happening:
[ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1067::process() timePosteriorCalculation=0.000373s [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1079::process() creating hypotheses... [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1093::process() Highest hypothesis=29, value=0.122176, timeHypothesesCreation=0.000012s [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1128::process() timeHypothesesValidation=0.000012s [DEBUG] (2015-03-31 18:20:17.063) Memory.cpp:1331::joinTrashThread() [DEBUG] (2015-03-31 18:20:17.063) Memory.cpp:1333::joinTrashThread() [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1154::process() Time emptying memory trash = 0.000029s, joining (actual overhead) = 0.000034s [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1166::process() Retrieving locations... around id=29 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1170::process() margin=17 maxRetieved=2 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1182::process() In TIME [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1189::process() neighbors of 29 in time = 19 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=29 m=0 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=28 m=1 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=30 m=1 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=27 m=2 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=31 m=2 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=26 m=3 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=32 m=3 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=25 m=4 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1217::process() nt=1 m=5 immunized=1 [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1233::process() In SPACE [DEBUG] (2015-03-31 18:20:17.063) Rtabmap.cpp:1240::process() neighbors of 29 in space = 19 [ INFO] (2015-03-31 18:20:17.063) Rtabmap.cpp:1290::process() margin=17, neighborhoodSize=17, reactivatedIds.size=9, nbLoadedFromDb=0, nbDirectNeighborsInDb=0, time=0.000177s (0.000000s 0.000000s) [DEBUG] (2015-03-31 18:20:17.063) Graph.cpp:1292::getNodesInRadius() Inlier 41: 0.003067 [DEBUG] (2015-03-31 18:20:17.063) Graph.cpp:1292::getNodesInRadius() Inlier 40: 0.005364 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 39: 0.007554 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 37: 0.008386 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 38: 0.008460 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 36: 0.009392 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 35: 0.011676 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 34: 0.014163 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 33: 0.017891 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 32: 0.019485 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 31: 0.019628 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 30: 0.019798 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 29: 0.021031 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 28: 0.023230 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 27: 0.026414 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 26: 0.029522 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 25: 0.033539 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1292::getNodesInRadius() Inlier 1: 0.034519 [DEBUG] (2015-03-31 18:20:17.064) Graph.cpp:1297::getNodesInRadius() found nodes=18 [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3888::reactivateSignatures() [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3904::reactivateSignatures() idsToLoad = 0 [DEBUG] (2015-03-31 18:20:17.064) DBDriver.cpp:303::loadSignatures() [DEBUG] (2015-03-31 18:20:17.064) DBDriver.cpp:343::loadSignatures() [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3780::enableWordsRef() size=0 [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3807::enableWordsRef() oldWordIds.size()=0, getOldIds time=0.000001s [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3816::enableWordsRef() loading words(0) time=0.000011s [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3879::enableWordsRef() 0 words total ref added from 0 signatures, time=0.000012s... [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:3920::reactivateSignatures() time = 0.000079s [ INFO] (2015-03-31 18:20:17.064) Rtabmap.cpp:1390::process() retrieval of 0 (db time = 0.000033s) [ INFO] (2015-03-31 18:20:17.064) Rtabmap.cpp:1393::process() total timeRetrievalDbAccess=0.000033s [ INFO] (2015-03-31 18:20:17.064) Rtabmap.cpp:1399::process() timeReactivations=0.000659s [DEBUG] (2015-03-31 18:20:17.064) Memory.cpp:1854::computeVisualTransform() Correspondences = 11 [DEBUG] (2015-03-31 18:20:17.064) util3d.cpp:1322::transformFromXYZCorrespondences() iterations=100 inlierThreshold=0.050000 [DEBUG] (2015-03-31 18:20:17.080) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 9/9, with an error threshold of 0.050000. [DEBUG] (2015-03-31 18:20:17.081) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.028959 (variance=0.000093) on iteration 0 out of 10. [DEBUG] (2015-03-31 18:20:17.081) util3d.cpp:1456::transformFromXYZCorrespondences() RANSAC inliers=9/11 tf=xyz=0.013163,0.022021,0.017044 rpy=0.026106,0.021489,0.030675 [DEBUG] (2015-03-31 18:20:17.083) Memory.cpp:2014::computeIcpTransform() Guess transform = xyz=0.013163,0.022021,0.017044 rpy=0.026106,0.021489,0.030675 [DEBUG] (2015-03-31 18:20:17.083) Memory.cpp:2135::computeIcpTransform() 2D ICP [ INFO] (2015-03-31 18:20:17.083) Memory.cpp:2143::computeIcpTransform() 2D ICP: Dropping z (0.017044), roll (0.026106) and pitch (0.021489) rotation! [DEBUG] (2015-03-31 18:20:17.105) Memory.cpp:2181::computeIcpTransform() hasConverged=true, variance=0.000003, correspondences=554/567 (97.707230%) [DEBUG] (2015-03-31 18:20:17.105) Memory.cpp:2219::computeIcpTransform() New transform = xyz=-0.021365,-0.014767,-0.000000 rpy=-0.000000,-0.000000,-0.031541 [ INFO] (2015-03-31 18:20:17.105) Memory.cpp:2355::addLink() old=29, new=42 transform: xyz=-0.021365,-0.014767,-0.000000 rpy=-0.000000,-0.000000,-0.031541 [DEBUG] (2015-03-31 18:20:17.105) Memory.cpp:2367::addLink() Add link between 29 and 42 [DEBUG] (2015-03-31 18:20:17.105) Signature.cpp:130::addLink() Add link 42 to 29 (type=1) [DEBUG] (2015-03-31 18:20:17.105) Signature.cpp:130::addLink() Add link 29 to 42 (type=1) [ INFO] (2015-03-31 18:20:17.105) Rtabmap.cpp:1519::process() timeAddLoopClosureLink=0.041177s [ INFO] (2015-03-31 18:20:17.105) Rtabmap.cpp:1579::process() timeLocalSpaceDetection=0.000014s [ INFO] (2015-03-31 18:20:17.105) Rtabmap.cpp:1593::process() Update map correction: SLAM mode [DEBUG] (2015-03-31 18:20:17.105) Rtabmap.cpp:2229::optimizeCurrentMap() Optimize map: around location 42 [DEBUG] (2015-03-31 18:20:17.105) Rtabmap.cpp:2234::optimizeCurrentMap() get ids=19 [ INFO] (2015-03-31 18:20:17.105) Rtabmap.cpp:2239::optimizeCurrentMap() get ids time 0.000042 s [DEBUG] (2015-03-31 18:20:17.105) Memory.cpp:3930::getMetricConstraints() [ INFO] (2015-03-31 18:20:17.105) Rtabmap.cpp:2244::optimizeCurrentMap() get constraints (19 poses, 19 edges) time 0.000134 s [DEBUG] (2015-03-31 18:20:17.105) Graph.cpp:219::optimize() Optimizing graph (pose=19 constraints=19)... [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:228::optimize() fill poses to TORO... [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:254::optimize() fill edges to TORO... [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:331::optimize() buildMST... [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:339::optimize() Building TORO tree... (if a crash happens just after this msg, TORO is not able to find the root of the graph!) [ INFO] (2015-03-31 18:20:17.106) Graph.cpp:352::optimize() TORO iterate begin (iterations=100) [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 0 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 1 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 2 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 3 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 4 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 5 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 6 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 7 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 8 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 9 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 10 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 11 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 12 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 13 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 14 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 15 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 16 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 17 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 18 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 19 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 20 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 21 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 22 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 23 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 24 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 25 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 26 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 27 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 28 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 29 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 30 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 31 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 32 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.106) Graph.cpp:391::optimize() iteration 33 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 34 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 35 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 36 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 37 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 38 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 39 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 40 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 41 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 42 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 43 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 44 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 45 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 46 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 47 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 48 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 49 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 50 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 51 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 52 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 53 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 54 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 55 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 56 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 57 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 58 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 59 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 60 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 61 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 62 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 63 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 64 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 65 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 66 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 67 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 68 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 69 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 70 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 71 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 72 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 73 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 74 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 75 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 76 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 77 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 78 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 79 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 80 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 81 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 82 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 83 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 84 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 85 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 86 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 87 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 88 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 89 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 90 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 91 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 92 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 93 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 94 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 95 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.107) Graph.cpp:391::optimize() iteration 96 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.108) Graph.cpp:391::optimize() iteration 97 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.108) Graph.cpp:391::optimize() iteration 98 global error=-nan error/constraint=-nan [DEBUG] (2015-03-31 18:20:17.108) Graph.cpp:391::optimize() iteration 99 global error=-nan error/constraint=-nan [ INFO] (2015-03-31 18:20:17.108) Graph.cpp:404::optimize() TORO iterate end [FATAL] (2015-03-31 18:20:17.108) Graph.cpp:415::optimize() Condition (!newPose.isNull()) not met! [Optimized pose 25 is null!?!?] ******* FATAL message occurred! Application will now exit. ******* rtabmap: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed. |
|
Administrator
|
To try on my side if it is a TORO problem or a machine problem, I need the graph. Can you add these lines at the beginning of the TOROOptimizer::optimizeGraph() method and copy/paste the resulting file here when rtabmap crashes?
saveGraph("toro_constraints.txt", poses, edgeConstraints);
UWARN("saved toro_constraints.txt");
Thank you, Mathieu |
|
|
Here is the output:
VERTEX3 1 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 VERTEX3 15 0.003454 -0.000052 0.000000 0.000000 0.000000 -0.052883 VERTEX3 16 0.006490 -0.000472 0.000000 0.000000 0.000000 -0.237190 VERTEX3 17 0.008903 -0.001309 0.000000 0.000000 0.000000 -0.435285 VERTEX3 18 0.011773 -0.003277 0.000000 0.000000 0.000000 -0.784176 VERTEX3 19 0.013268 -0.005339 0.000000 0.000000 0.000000 -1.131322 VERTEX3 20 0.013621 -0.006430 0.000000 0.000000 0.000000 -1.499587 VERTEX3 21 0.013545 -0.007444 0.000000 0.000000 0.000000 -1.850573 VERTEX3 22 0.013623 -0.007294 0.000000 0.000000 0.000000 -2.309769 VERTEX3 23 0.015563 -0.005929 0.000000 0.000000 0.000000 -2.721841 VERTEX3 24 0.018391 -0.005427 0.000000 0.000000 -0.000000 3.105464 VERTEX3 25 0.021205 -0.006085 0.000000 0.000000 -0.000000 2.737025 VERTEX3 26 0.023174 -0.007265 0.000000 0.000000 -0.000000 2.448872 VERTEX3 27 0.024574 -0.009013 0.000000 0.000000 -0.000000 2.023884 VERTEX3 28 0.024679 -0.009158 0.000000 0.000000 -0.000000 1.653001 VERTEX3 29 0.024697 -0.009117 0.000000 0.000000 -0.000000 1.275836 VERTEX3 30 0.024569 -0.009193 0.000000 0.000000 -0.000000 0.927468 VERTEX3 31 0.025978 -0.007785 0.000000 0.000000 -0.000000 0.586780 VERTEX3 32 0.028372 -0.006791 0.000000 0.000000 -0.000000 0.266163 VERTEX3 33 0.030266 -0.006502 0.000000 0.000000 -0.000000 0.016755 VERTEX3 34 0.033701 -0.006807 0.000000 0.000000 0.000000 -0.203680 VERTEX3 35 0.035660 -0.007390 0.000000 0.000000 0.000000 -0.388336 VERTEX3 36 0.037548 -0.008373 0.000000 0.000000 0.000000 -0.626050 VERTEX3 37 0.039672 -0.010508 0.000000 0.000000 0.000000 -0.926770 VERTEX3 38 0.040021 -0.011189 0.000000 0.000000 0.000000 -1.196772 EDGE3 1 15 0.003454 -0.000052 0.000000 0.000000 -0.000000 -0.052883 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 15 16 0.003054 -0.000258 0.000000 0.000000 -0.000000 -0.184307 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 16 17 0.002543 -0.000247 0.000000 0.000000 -0.000000 -0.198095 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 17 18 0.003432 -0.000574 0.000000 0.000000 -0.000000 -0.348891 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 18 19 0.002514 -0.000404 0.000000 0.000000 -0.000000 -0.347146 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 19 20 0.001138 -0.000145 0.000000 0.000000 -0.000000 -0.368264 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 20 21 0.001006 -0.000147 0.000000 0.000000 0.000000 -0.350986 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 21 22 -0.000166 0.000034 0.000000 0.000000 -0.000000 -0.459196 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 22 23 -0.002315 0.000514 0.000000 0.000000 -0.000000 -0.412072 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 23 24 -0.002788 0.000694 0.000000 0.000000 -0.000000 -0.455880 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 24 25 -0.002836 0.000556 0.000000 0.000000 -0.000000 -0.368439 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 25 26 -0.002274 0.000310 0.000000 0.000000 -0.000000 -0.288154 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 26 27 -0.002194 0.000451 0.000000 0.000000 -0.000000 -0.424988 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 27 28 -0.000176 -0.000031 0.000000 0.000000 -0.000000 -0.370882 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 28 29 0.000040 -0.000021 0.000000 0.000000 -0.000000 -0.377166 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 29 30 -0.000110 0.000100 0.000000 0.000000 -0.000000 -0.348368 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 30 31 0.001972 -0.000282 0.000000 0.000000 -0.000000 -0.340688 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 31 32 0.002544 -0.000498 0.000000 0.000000 -0.000000 -0.320617 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 32 33 0.001903 -0.000219 0.000000 0.000000 -0.000000 -0.249408 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 33 34 0.003430 -0.000363 0.000000 0.000000 -0.000000 -0.220435 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 34 35 0.002036 -0.000174 0.000000 0.000000 -0.000000 -0.184656 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 35 36 0.002119 -0.000195 0.000000 0.000000 -0.000000 -0.237714 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 36 37 0.002973 -0.000486 0.000000 0.000000 -0.000000 -0.300720 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 37 38 0.000754 -0.000130 0.000000 0.000000 -0.000000 -0.270002 0.000000 0 0 0 0 0 0.000000 0 0 0 0 0.000000 0 0 0 0.000000 0 0 0.000000 0 0.000000 EDGE3 38 19 -0.017083 -0.026207 -0.000000 -0.000000 -0.000000 0.043848 740085.250000 0 0 0 0 0 740085.250000 0 0 0 0 740085.250000 0 0 0 740085.250000 0 0 740085.250000 0 740085.250000 |
|
Administrator
|
Thank you,
For some reasons, the information matrix of the neighbor links (odometry) is null. I verified to rtabmap_ros package and there is supposed to be a check if the covariance matrix of the odometry message is null (setting to 1 by default). I added more asserts, can you try again (update code) and show the debug log? Mathieu |
|
Administrator
|
Also, are you on 32 or 64 bits OS?
|
|
|
It's a 64 bit os. Here is the log:
[ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1067::process() timePosteriorCalculation=0.000640s [ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1079::process() creating hypotheses... [ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1093::process() Highest hypothesis=70, value=0.166064, timeHypothesesCreation=0.000011s [ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1128::process() timeHypothesesValidation=0.000020s [DEBUG] (2015-04-02 10:12:28.556) Memory.cpp:1331::joinTrashThread() [DEBUG] (2015-04-02 10:12:28.556) Memory.cpp:1333::joinTrashThread() [ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1154::process() Time emptying memory trash = 0.000029s, joining (actual overhead) = 0.000056s [ INFO] (2015-04-02 10:12:28.556) Rtabmap.cpp:1166::process() Retrieving locations... around id=70 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1170::process() margin=17 maxRetieved=2 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1182::process() In TIME [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1189::process() neighbors of 70 in time = 18 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=70 m=0 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=1 m=1 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=71 m=1 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=72 m=2 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=73 m=3 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=74 m=4 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=75 m=5 immunized=1 [DEBUG] (2015-04-02 10:12:28.556) Rtabmap.cpp:1217::process() nt=76 m=6 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=77 m=7 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=78 m=8 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=79 m=9 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=80 m=10 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=81 m=11 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=82 m=12 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=83 m=13 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=84 m=14 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=85 m=15 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1217::process() nt=86 m=16 immunized=1 [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1233::process() In SPACE [DEBUG] (2015-04-02 10:12:28.557) Rtabmap.cpp:1240::process() neighbors of 70 in space = 18 [ INFO] (2015-04-02 10:12:28.557) Rtabmap.cpp:1290::process() margin=17, neighborhoodSize=17, reactivatedIds.size=18, nbLoadedFromDb=0, nbDirectNeighborsInDb=0, time=0.000261s (0.000000s 0.000000s) [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 100: 0.000514 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 99: 0.001322 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 98: 0.002536 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 97: 0.003099 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 96: 0.003520 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 95: 0.004856 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 94: 0.005494 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 93: 0.006811 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 73: 0.007056 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 74: 0.007167 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 72: 0.007213 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 71: 0.007679 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 76: 0.007799 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 86: 0.007844 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 85: 0.007862 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 87: 0.007914 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 83: 0.007930 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 84: 0.007941 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 88: 0.007977 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 75: 0.007983 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 92: 0.008062 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 90: 0.008069 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 89: 0.008222 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 91: 0.008300 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 78: 0.008397 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 82: 0.008615 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 77: 0.008659 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 81: 0.008663 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 70: 0.009600 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 79: 0.009714 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 80: 0.009848 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1303::getNodesInRadius() Inlier 1: 0.011032 [DEBUG] (2015-04-02 10:12:28.557) Graph.cpp:1308::getNodesInRadius() found nodes=32 [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3888::reactivateSignatures() [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3904::reactivateSignatures() idsToLoad = 0 [DEBUG] (2015-04-02 10:12:28.557) DBDriver.cpp:303::loadSignatures() [DEBUG] (2015-04-02 10:12:28.557) DBDriver.cpp:343::loadSignatures() [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3780::enableWordsRef() size=0 [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3807::enableWordsRef() oldWordIds.size()=0, getOldIds time=0.000000s [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3816::enableWordsRef() loading words(0) time=0.000011s [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3879::enableWordsRef() 0 words total ref added from 0 signatures, time=0.000011s... [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:3920::reactivateSignatures() time = 0.000085s [ INFO] (2015-04-02 10:12:28.557) Rtabmap.cpp:1390::process() retrieval of 0 (db time = 0.000031s) [ INFO] (2015-04-02 10:12:28.557) Rtabmap.cpp:1393::process() total timeRetrievalDbAccess=0.000031s [ INFO] (2015-04-02 10:12:28.557) Rtabmap.cpp:1399::process() timeReactivations=0.000904s [DEBUG] (2015-04-02 10:12:28.557) Memory.cpp:1854::computeVisualTransform() Correspondences = 42 [DEBUG] (2015-04-02 10:12:28.557) util3d.cpp:1322::transformFromXYZCorrespondences() iterations=100 inlierThreshold=0.050000 [DEBUG] (2015-04-02 10:12:28.571) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 25/25, with an error threshold of 0.050000. [DEBUG] (2015-04-02 10:12:28.572) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.050000 (variance=0.000630) on iteration 0 out of 10. [DEBUG] (2015-04-02 10:12:28.572) util3d.cpp:1456::transformFromXYZCorrespondences() RANSAC inliers=25/42 tf=xyz=0.024092,-0.038276,0.009060 rpy=0.001730,0.008428,0.382084 [DEBUG] (2015-04-02 10:12:28.574) Memory.cpp:2014::computeIcpTransform() Guess transform = xyz=0.024092,-0.038276,0.009060 rpy=0.001730,0.008428,0.382084 [DEBUG] (2015-04-02 10:12:28.574) Memory.cpp:2135::computeIcpTransform() 2D ICP [ INFO] (2015-04-02 10:12:28.574) Memory.cpp:2143::computeIcpTransform() 2D ICP: Dropping z (0.009060), roll (0.001730) and pitch (0.008428) rotation! [DEBUG] (2015-04-02 10:12:28.587) Memory.cpp:2181::computeIcpTransform() hasConverged=true, variance=0.000166, correspondences=314/507 (56.884056%) [DEBUG] (2015-04-02 10:12:28.587) Memory.cpp:2219::computeIcpTransform() New transform = xyz=-0.010780,0.099970,-0.000000 rpy=-0.000000,0.000000,-0.412228 [ INFO] (2015-04-02 10:12:28.588) Memory.cpp:2355::addLink() old=70, new=101 transform: xyz=-0.010780,0.099970,-0.000000 rpy=-0.000000,0.000000,-0.412228 [DEBUG] (2015-04-02 10:12:28.588) Memory.cpp:2367::addLink() Add link between 70 and 101 [DEBUG] (2015-04-02 10:12:28.588) Signature.cpp:130::addLink() Add link 101 to 70 (type=1) [DEBUG] (2015-04-02 10:12:28.588) Signature.cpp:130::addLink() Add link 70 to 101 (type=1) [ INFO] (2015-04-02 10:12:28.588) Rtabmap.cpp:1519::process() timeAddLoopClosureLink=0.030331s [ INFO] (2015-04-02 10:12:28.588) Rtabmap.cpp:1579::process() timeLocalSpaceDetection=0.000012s [ INFO] (2015-04-02 10:12:28.588) Rtabmap.cpp:1593::process() Update map correction: SLAM mode [DEBUG] (2015-04-02 10:12:28.588) Rtabmap.cpp:2229::optimizeCurrentMap() Optimize map: around location 101 [DEBUG] (2015-04-02 10:12:28.588) Rtabmap.cpp:2234::optimizeCurrentMap() get ids=33 [ INFO] (2015-04-02 10:12:28.588) Rtabmap.cpp:2239::optimizeCurrentMap() get ids time 0.000062 s [DEBUG] (2015-04-02 10:12:28.588) Memory.cpp:3930::getMetricConstraints() [ INFO] (2015-04-02 10:12:28.588) Rtabmap.cpp:2244::optimizeCurrentMap() get constraints (33 poses, 33 edges) time 0.000218 s [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:219::optimize() Optimizing graph (pose=33 constraints=33)... [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:228::optimize() fill poses to TORO... [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:254::optimize() fill edges to TORO... [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:331::optimize() buildMST... [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:339::optimize() Building TORO tree... (if a crash happens just after this msg, TORO is not able to find the root of the graph!) [ INFO] (2015-04-02 10:12:28.588) Graph.cpp:352::optimize() TORO iterate begin (iterations=100) [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 0 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 1 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 2 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 3 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 4 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 5 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 6 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 7 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 8 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 9 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 10 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 11 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 12 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 13 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 14 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 15 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 16 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 17 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.588) Graph.cpp:391::optimize() iteration 18 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 19 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 20 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 21 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 22 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 23 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 24 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 25 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 26 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 27 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 28 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 29 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 30 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 31 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 32 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 33 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 34 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 35 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 36 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 37 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 38 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 39 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 40 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 41 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 42 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 43 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 44 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 45 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 46 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 47 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 48 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 49 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 50 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 51 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 52 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 53 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 54 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 55 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 56 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 57 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 58 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 59 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 60 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 61 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 62 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 63 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 64 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 65 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 66 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 67 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 68 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 69 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 70 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 71 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.589) Graph.cpp:391::optimize() iteration 72 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 73 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 74 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 75 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 76 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 77 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 78 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 79 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 80 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 81 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 82 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 83 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 84 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 85 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 86 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 87 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 88 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 89 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 90 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 91 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 92 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 93 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 94 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 95 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 96 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 97 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 98 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 10:12:28.590) Graph.cpp:391::optimize() iteration 99 global error=-nan error/constraint=-nan [ INFO] (2015-04-02 10:12:28.590) Graph.cpp:404::optimize() TORO iterate end [FATAL] (2015-04-02 10:12:28.590) Graph.cpp:415::optimize() Condition (!newPose.isNull()) not met! [Optimized pose 70 is null!?!?] ******* FATAL message occurred! Application will now exit. ******* rtabmap: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed. |
|
Administrator
|
Thank you. The last thing is to show exactly what is fed to TORO with the covariance used. Can you add these lines at the beginning of the TOROOptimizer::optimize(...) function? then show the logs with all these warnings.
UWARN("Poses:");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UWARN("Pose %d t=%s", iter->first, iter->second.prettyPrint().c_str());
}
UWARN("Links:");
for(std::map<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
const Link & l = iter->second;
UWARN("Link %d->%d t=%s rotVar=%f transVar=%f", l.from(), l.to(), l.transform().prettyPrint().c_str(), l.rotVariance(), l.transVariance());
}
I was wrong, small values in the information matrices means that the covariance is very HIGH. Normally, the variance values in the links should be between >0 and 1 (these values come from the covariance matrix of the odometry). |
|
Administrator
|
You could try to ignore the covariance too (though it is just avoiding the real problem if it there is a wrong covariance), by setting this parameter under the rtabmap node:
<param name="RGBD/OptimizeVarianceIgnored" type="string" value="true"/> |
|
|
In reply to this post by matlabbe
Here it is:
[ INFO] (2015-04-02 17:00:39.447) Rtabmap.cpp:1390::process() retrieval of 0 (db time = 0.000032s) [ INFO] (2015-04-02 17:00:39.447) Rtabmap.cpp:1393::process() total timeRetrievalDbAccess=0.000032s [ INFO] (2015-04-02 17:00:39.447) Rtabmap.cpp:1399::process() timeReactivations=0.000738s [DEBUG] (2015-04-02 17:00:39.447) Memory.cpp:1854::computeVisualTransform() Correspondences = 53 [DEBUG] (2015-04-02 17:00:39.447) util3d.cpp:1322::transformFromXYZCorrespondences() iterations=100 inlierThreshold=0.050000 [DEBUG] (2015-04-02 17:00:39.464) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 39/37, with an error threshold of 0.050000. [DEBUG] (2015-04-02 17:00:39.464) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.050000 (variance=0.000571) on iteration 0 out of 10. [DEBUG] (2015-04-02 17:00:39.464) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 37/38, with an error threshold of 0.050000. [DEBUG] (2015-04-02 17:00:39.465) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.050000 (variance=0.000509) on iteration 1 out of 10. [DEBUG] (2015-04-02 17:00:39.465) util3d.cpp:1371::transformFromXYZCorrespondences() RANSAC refineModel: Number of inliers found (before/after): 38/38, with an error threshold of 0.050000. [DEBUG] (2015-04-02 17:00:39.465) util3d.cpp:1388::transformFromXYZCorrespondences() RANSAC refineModel: New estimated error threshold: 0.050000 (variance=0.000497) on iteration 2 out of 10. [DEBUG] (2015-04-02 17:00:39.465) util3d.cpp:1456::transformFromXYZCorrespondences() RANSAC inliers=38/53 tf=xyz=0.006182,-0.027207,0.013288 rpy=0.004617,0.011210,0.258372 [DEBUG] (2015-04-02 17:00:39.466) Memory.cpp:2014::computeIcpTransform() Guess transform = xyz=0.006182,-0.027207,0.013288 rpy=0.004617,0.011210,0.258372 [DEBUG] (2015-04-02 17:00:39.466) Memory.cpp:2135::computeIcpTransform() 2D ICP [ INFO] (2015-04-02 17:00:39.466) Memory.cpp:2143::computeIcpTransform() 2D ICP: Dropping z (0.013288), roll (0.004617) and pitch (0.011210) rotation! [DEBUG] (2015-04-02 17:00:39.523) Memory.cpp:2181::computeIcpTransform() hasConverged=true, variance=0.001409, correspondences=347/531 (65.348396%) [DEBUG] (2015-04-02 17:00:39.523) Memory.cpp:2219::computeIcpTransform() New transform = xyz=-0.022747,0.043094,-0.000000 rpy=-0.000000,0.000000,-0.261506 [ INFO] (2015-04-02 17:00:39.523) Memory.cpp:2355::addLink() old=22, new=43 transform: xyz=-0.022747,0.043094,-0.000000 rpy=-0.000000,0.000000,-0.261506 [DEBUG] (2015-04-02 17:00:39.523) Memory.cpp:2367::addLink() Add link between 22 and 43 [DEBUG] (2015-04-02 17:00:39.523) Signature.cpp:130::addLink() Add link 43 to 22 (type=1) [DEBUG] (2015-04-02 17:00:39.523) Signature.cpp:130::addLink() Add link 22 to 43 (type=1) [ INFO] (2015-04-02 17:00:39.523) Rtabmap.cpp:1519::process() timeAddLoopClosureLink=0.075804s [ INFO] (2015-04-02 17:00:39.523) Rtabmap.cpp:1579::process() timeLocalSpaceDetection=0.000012s [ INFO] (2015-04-02 17:00:39.523) Rtabmap.cpp:1593::process() Update map correction: SLAM mode [DEBUG] (2015-04-02 17:00:39.523) Rtabmap.cpp:2229::optimizeCurrentMap() Optimize map: around location 43 [DEBUG] (2015-04-02 17:00:39.523) Rtabmap.cpp:2234::optimizeCurrentMap() get ids=23 [ INFO] (2015-04-02 17:00:39.523) Rtabmap.cpp:2239::optimizeCurrentMap() get ids time 0.000043 s [DEBUG] (2015-04-02 17:00:39.523) Memory.cpp:3930::getMetricConstraints() [ INFO] (2015-04-02 17:00:39.523) Rtabmap.cpp:2244::optimizeCurrentMap() get constraints (23 poses, 23 edges) time 0.000124 s [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:218::optimize() Poses: [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 1 t=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 22 t=xyz=0.001236,-0.000018,0.000000 rpy=0.000000,0.000000,-0.051662 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 23 t=xyz=0.003335,-0.000373,0.000000 rpy=0.000000,0.000000,-0.279427 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 24 t=xyz=0.004753,-0.000949,0.000000 rpy=0.000000,0.000000,-0.546812 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 25 t=xyz=0.005368,-0.001473,0.000000 rpy=0.000000,0.000000,-0.854862 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 26 t=xyz=0.005455,-0.001670,0.000000 rpy=0.000000,0.000000,-1.293463 [ WARN] (2015-04-02 17:00:39.523) Graph.cpp:221::optimize() Pose 27 t=xyz=0.005468,-0.001668,0.000000 rpy=0.000000,0.000000,-1.649162 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 28 t=xyz=0.005578,-0.001302,0.000000 rpy=0.000000,0.000000,-1.975189 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 29 t=xyz=0.005532,-0.001272,0.000000 rpy=0.000000,0.000000,-2.300868 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 30 t=xyz=0.005538,-0.001278,0.000000 rpy=0.000000,0.000000,-2.590418 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 31 t=xyz=0.005539,-0.001282,0.000000 rpy=0.000000,0.000000,-2.863911 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 32 t=xyz=0.005956,-0.001205,0.000000 rpy=0.000000,-0.000000,3.138626 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 33 t=xyz=0.006218,-0.001181,0.000000 rpy=0.000000,-0.000000,2.873161 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 34 t=xyz=0.006018,-0.001109,0.000000 rpy=0.000000,-0.000000,2.647490 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 35 t=xyz=0.005556,-0.000803,0.000000 rpy=0.000000,-0.000000,2.407333 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 36 t=xyz=0.004578,0.000376,0.000000 rpy=0.000000,-0.000000,2.154958 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 37 t=xyz=0.004006,0.001558,0.000000 rpy=0.000000,-0.000000,1.818459 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 38 t=xyz=0.003806,0.003463,0.000000 rpy=0.000000,-0.000000,1.552121 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 39 t=xyz=0.004072,0.005360,0.000000 rpy=0.000000,-0.000000,1.313884 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 40 t=xyz=0.004494,0.006382,0.000000 rpy=0.000000,-0.000000,0.962724 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 41 t=xyz=0.005174,0.007201,0.000000 rpy=0.000000,-0.000000,0.683122 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 42 t=xyz=0.005767,0.007541,0.000000 rpy=0.000000,-0.000000,0.452040 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:221::optimize() Pose 43 t=xyz=0.006986,0.007918,0.000000 rpy=0.000000,-0.000000,0.182387 [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:223::optimize() Links: [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 1->22 t=xyz=0.001236,-0.000018,0.000000 rpy=0.000000,-0.000000,-0.051662 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 22->23 t=xyz=0.002114,-0.000246,0.000000 rpy=0.000000,-0.000000,-0.227765 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 23->24 t=xyz=0.001523,-0.000163,0.000000 rpy=0.000000,-0.000000,-0.267384 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 24->25 t=xyz=0.000797,-0.000128,0.000000 rpy=0.000000,-0.000000,-0.308051 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 25->26 t=xyz=0.000205,-0.000064,0.000000 rpy=0.000000,-0.000000,-0.438601 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 26->27 t=xyz=0.000002,0.000013,0.000000 rpy=0.000000,0.000000,-0.355698 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 27->28 t=xyz=-0.000374,0.000082,0.000000 rpy=0.000000,-0.000000,-0.326028 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 28->29 t=xyz=-0.000010,-0.000055,0.000000 rpy=0.000000,-0.000000,-0.325678 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 29->30 t=xyz=0.000001,0.000008,0.000000 rpy=0.000000,-0.000000,-0.289550 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 30->31 t=xyz=0.000001,0.000004,0.000000 rpy=0.000000,-0.000000,-0.273493 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 31->32 t=xyz=-0.000422,0.000040,0.000000 rpy=0.000000,-0.000000,-0.280649 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 32->33 t=xyz=-0.000262,-0.000025,0.000000 rpy=0.000000,-0.000000,-0.265465 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 33->34 t=xyz=0.000212,-0.000017,0.000000 rpy=0.000000,-0.000000,-0.225671 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 34->35 t=xyz=0.000551,-0.000050,0.000000 rpy=0.000000,-0.000000,-0.240157 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 35->36 t=xyz=0.001516,-0.000220,0.000000 rpy=0.000000,-0.000000,-0.252375 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 36->37 t=xyz=0.001302,-0.000174,0.000000 rpy=0.000000,-0.000000,-0.336499 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 37->38 t=xyz=0.001896,-0.000274,0.000000 rpy=0.000000,-0.000000,-0.266337 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 38->39 t=xyz=0.001902,-0.000231,0.000000 rpy=0.000000,-0.000000,-0.238237 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 39->40 t=xyz=0.001096,-0.000148,0.000000 rpy=0.000000,-0.000000,-0.351160 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 40->41 t=xyz=0.001061,-0.000090,0.000000 rpy=0.000000,-0.000000,-0.279602 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 41->42 t=xyz=0.000674,-0.000111,0.000000 rpy=0.000000,-0.000000,-0.231082 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 42->43 t=xyz=0.001262,-0.000193,0.000000 rpy=0.000000,-0.000000,-0.269653 rotVar=inf transVar=inf [ WARN] (2015-04-02 17:00:39.524) Graph.cpp:227::optimize() Link 43->22 t=xyz=-0.022747,0.043094,-0.000000 rpy=-0.000000,0.000000,-0.261506 rotVar=0.001409 transVar=0.001409 [DEBUG] (2015-04-02 17:00:39.524) Graph.cpp:231::optimize() Optimizing graph (pose=23 constraints=23)... [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:240::optimize() fill poses to TORO... [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:266::optimize() fill edges to TORO... [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:343::optimize() buildMST... [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:351::optimize() Building TORO tree... (if a crash happens just after this msg, TORO is not able to find the root of the graph!) [ INFO] (2015-04-02 17:00:39.526) Graph.cpp:364::optimize() TORO iterate begin (iterations=100) [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 0 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 1 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 2 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 3 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 4 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 5 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 6 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 7 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 8 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 9 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 10 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 11 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 12 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 13 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 14 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 15 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.526) Graph.cpp:403::optimize() iteration 16 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 17 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 18 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 19 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 20 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 21 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 22 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 23 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 24 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 25 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 26 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 27 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 28 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 29 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 30 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 31 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 32 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 33 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 34 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 35 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 36 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 37 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 38 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 39 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 40 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 41 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 42 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 43 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 44 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 45 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 46 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 47 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 48 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 49 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 50 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 51 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 52 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 53 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 54 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 55 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 56 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 57 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 58 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 59 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 60 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 61 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 62 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 63 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 64 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 65 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 66 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 67 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 68 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 69 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 70 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 71 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 72 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 73 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 74 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 75 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 76 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 77 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.527) Graph.cpp:403::optimize() iteration 78 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 79 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 80 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 81 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 82 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 83 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 84 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 85 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 86 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 87 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 88 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 89 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 90 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 91 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 92 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 93 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 94 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 95 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 96 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 97 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 98 global error=-nan error/constraint=-nan [DEBUG] (2015-04-02 17:00:39.528) Graph.cpp:403::optimize() iteration 99 global error=-nan error/constraint=-nan [ INFO] (2015-04-02 17:00:39.528) Graph.cpp:416::optimize() TORO iterate end [FATAL] (2015-04-02 17:00:39.528) Graph.cpp:427::optimize() Condition (!newPose.isNull()) not met! [Optimized pose 22 is null!?!?] ******* FATAL message occurred! Application will now exit. ******* rtabmap: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed. |
|
|
In reply to this post by matlabbe
With "RGBD/OptimizeVarianceIgnored" enabled, the error disappears.
|
|
Administrator
|
Yes, the problem was the inf variance. This comes from the odometry messages. I added a check in rtabmap node so now it is ignored when infinity values are detected. You can update to latest rtabmap_ros code, it should fix the problem.
Thank you to help debug it. cheers, Mathieu |
«
Return to Official RTAB-Map Forum
|
1 view|%1 views
| Free forum by Nabble | Edit this page |