Official RTAB-Map Forum

Icp Odometry

classic Classic list List threaded Threaded
♦
♦
 5 messages Options
bruchpilot123
Reply | Threaded
Open this post in threaded view
♦
♦
|

Icp Odometry

bruchpilot123
Hello, i am using an 3D Blickfeld Lidar and a ZED Stereo Camera on a drone. The Hardware is the Nvidia Jetson. Actually it seems to be that i have issues, with the icp odometry. As far as the drone is on the ground, icp odometry is working without any issues. When moving the drone, i get the following warnings:

[ERROR] (2021-07-30 13:53:28.066) RegistrationIcp.cpp:897::computeTransformationImpl() RegistrationIcp cannot do registration with a null guess.
[ WARN] (2021-07-30 13:53:28.066) OdometryF2M.cpp:525::computeTransform() Registration failed: "RegistrationIcp cannot do registration with a null guess."

From that point, odometry fails. And rtabmap fails. This seems only to happen at movements with height and left/right. if i just turn the  lidar left and right, nothing seems to happen.

Following launch File I am using: 

<?xml version="1.0"?>
<launch>
    <arg name="rtabmapviz" default="true" />
    <node name="rtabmap" pkg="rtabmap_ros" type="icp_odometry" args="" output="screen">
        <remap from="/scan_cloud" to="/bf_lidar/points_raw"/>
        <param name="scan_cloud_max_points" type="int" value="0"/>
        <param name="scan_downsampling_step" type="int" value="1"/>
        <param name="scan_normal_k" type="int" value="0"/>
        <!--<param name="Odom/ResetCountdown" type="int" value="1"/>-->
        <!-- ICP parameters -->
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
        <param name="Icp/DownsamplingStep"    type="string" value="1"/> 
        <param name="Icp/Epsilon"             type="string" value="0.001"/>
        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
        <param name="Icp/MaxTranslation"      type="string" value="2"/>
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
        <param name="Icp/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>

        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
    </node>
</launch>
If i uncomment the reset countdown, the results for rtabmap are getting worse.

For rtabmap, I use the following launch file: 
<?xml version="1.0"?>
<launch>
    <arg name="rtabmapviz" default="true" />
    <node name="rtabmap_slam" pkg="rtabmap_ros" type="rtabmap" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini --delete_db_on_start --udebug" output="screen">
        <remap from="/scan_cloud" to="/bf_lidar/points_raw"/>
        <remap from="/rgb/image" to="/zedA/zed_node_A/rgb/image_rect_color"/>
        <remap from="/rgb/camera_info" to="/zedA/zed_node_A/rgb/camera_info"/>
        <!--<remap from="/odom" to= "/zedA/zed_node_A/odom"/>-->

        <param name="Optimizer/Iterations" type="int" value="50"/>
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="subscribe_rgbd"       type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        <param name="odom_sensor_sync"     type="bool" value="true"/>


        <param name="Grid/FromDepth" type="string" value="false"/>
        <param name="queue_size" type="int" value="10"/>
        <param name="gen_scan_max_depth" type="double" value="100" />
        <param name="max_depth" type="double" value="100.0"/>
        <param name="map_always_update" type="bool" value="false"/>
    </node>
</launch>

Sadly I can not use the database standalone viewer, because of missing qt6 libraries and also your jetson fix did not solve the error in my case. Anyway, i uploaded the Db. rtabmap.db

I really appreciate any help. I do not know what to do anymore. From my perspective i see no reason, why it should not work... If following information is required, please just say.

Have a nice day everyone.

bruchpilot123
bruchpilot123
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Icp Odometry

bruchpilot123
Hey, is there nobody that could help or might have any idea?

Sorry for the inconveniences

Kindly regards,

Robert Scheffler
bruchpilot123
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Icp Odometry

bruchpilot123
Hey,

i totally changed my approach. I am now trying to get odometry from ZED Data. But it is not working, somehow.  I please ask for to help, since I really do not know what to do. Following launch File am I using: 

<?xml version="1.0"?>
<launch>
    <arg name="rtabmapviz" default="true" />
    <node name="rtabmap_slam" pkg="rtabmap_ros" type="rtabmap" args="--delete_db_on_start --udebug" output="screen">
        <remap from="/scan_cloud" to="/bf_lidar/points_raw"/>
        <remap from="/rgb/image" to="/zed2/zed_node/rgb/image_rect_color"/>
        <remap from="/rgb/camera_info" to="/zed2/zed_node/rgb/camera_info"/>
        <remap from="/odom" to= "/zed2/zed_node/odom"/>
        <remap from="/depth/image" to= "/zed2/zed_node/depth/depth_registered"/>

        <param name="Optimizer/Iterations" type="int" value="50"/>
        <param name="subscribe_depth"      type="bool" value="true"/>
        <param name="subscribe_scan_cloud" type="bool" value="false"/>
        <param name="subscribe_rgbd"       type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="true"/>
        <param name="odom_sensor_sync"     type="bool" value="true"/>

        <param name="publish_tf"        type="bool" value="true"/>
        <param name="Grid/FromDepth" type="string" value="true"/>
        <param name="queue_size" type="int" value="10"/>
        <param name="gen_scan_max_depth" type="double" value="100" />
        <param name="max_depth" type="double" value="100.0"/>
        <param name="map_always_update" type="bool" value="false"/>
        <param name="subscribe_scan" type="bool" value="false"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="odom_tf_angular_variance" type="double" value="0.001"/>
        <param name="odom_tf_linear_variance" type="double" value="0.001"/>
    </node>
</launch>

Somehome, the results are still not useful. I tried to map a wall and in my map a several walls. Does anybody have any suggestion? Following log in rtabmap: Is it a problem, that i assemble 0 obstacles? I do not see the point, wh it is not working. Also indoors i did not make it to work.

[ INFO] [1628175733.034806912]: Octomap update time = 0.000135s
[ INFO] [1628175733.034949664]: Assembled 0 obstacle and 0 ground clouds (9927 points, 0.000006s)
[ INFO] [1628175733.035107008]: rtabmap (414): Rate=1.00s, Limit=0.000s, Conversion=0.0082s, RTAB-Map=0.1967s, Maps update=0.0257s pub=0.0001s (local map=2, WM=2)
[DEBUG] (2021-08-05 17:02:13.879) Rtabmap.cpp:1008::process()
[ INFO] (2021-08-05 17:02:13.880) Rtabmap.cpp:1068::process() getting data...
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:2478::getLastWorkingSignature()
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:2478::getLastWorkingSignature()
[ INFO] (2021-08-05 17:02:13.880) Rtabmap.cpp:1213::process() Updating memory...
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:773::update()
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:782::update() pre-updating...
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:5429::cleanUnusedWords() Removing 122 words (dictionary size=628)...
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:786::update() time preUpdate=0.261068 ms
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:4117::createSignature()
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:4280::createSignature() Start dictionary update thread
[DEBUG] (2021-08-05 17:02:13.880) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-08-05 17:02:13.880) VWDictionary.cpp:685::update() Dictionary has not changed, so no need to update it! (size=0)
[DEBUG] (2021-08-05 17:02:13.880) VWDictionary.cpp:689::update()
[DEBUG] (2021-08-05 17:02:13.880) Memory.cpp:4288::createSignature() Received kpts=0 kpts3D=0, descriptors=0 _useOdometryFeatures=true
[ INFO] (2021-08-05 17:02:13.881) Memory.cpp:4337::createSignature() Extract features
[DEBUG] (2021-08-05 17:02:13.881) Memory.cpp:4386::createSignature() rawDescriptorsKept=1, pose=1, maxFeatures=500, visMaxFeatures=1000
[DEBUG] (2021-08-05 17:02:13.881) Memory.cpp:4391::createSignature() Changing temporary max features from 500 to 1000
[DEBUG] (2021-08-05 17:02:13.885) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-08-05 17:02:13.885) util2d.cpp:1163::computeRoi() roi = 0, 0, 1280, 720
[DEBUG] (2021-08-05 17:02:13.885) util2d.cpp:1190::computeRoi() roi = 0, 0, 1280, 720
[DEBUG] (2021-08-05 17:02:13.930) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.045491 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-08-05 17:02:13.930) Memory.cpp:4407::createSignature() time keypoints (1000) = 0.050096s
[DEBUG] (2021-08-05 17:02:13.940) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 955, remaining kpts=955
[DEBUG] (2021-08-05 17:02:13.940) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 955, remaining kpts=955
[DEBUG] (2021-08-05 17:02:13.940) Memory.cpp:4413::createSignature() time descriptors (955) = 0.010226s
[DEBUG] (2021-08-05 17:02:13.941) Memory.cpp:4415::createSignature() ratio=0.500000, meanWordsPerLocation=500
[DEBUG] (2021-08-05 17:02:13.941) Memory.cpp:4561::createSignature() time keypoints 3D (955) = 0.000816s
[DEBUG] (2021-08-05 17:02:13.941) Memory.cpp:4644::createSignature() Joining dictionary update thread...
[DEBUG] (2021-08-05 17:02:13.941) Memory.cpp:4646::createSignature() Joining dictionary update thread... thread finished!
[DEBUG] (2021-08-05 17:02:13.941) Memory.cpp:4653::createSignature() time descriptor and memory update (955 of size=32) = 0.000095s
[DEBUG] (2021-08-05 17:02:13.941) Features2d.cpp:350::limitKeypoints() too much words (955), removing words with the hessian threshold
[DEBUG] (2021-08-05 17:02:13.942) Features2d.cpp:371::limitKeypoints() 455 keypoints removed, (kept 500), minimum response=0.000000
[DEBUG] (2021-08-05 17:02:13.942) Features2d.cpp:372::limitKeypoints() filter keypoints time = 0.000347 s
[DEBUG] (2021-08-05 17:02:13.942) VWDictionary.cpp:763::addNewWords() id=415 descriptors=500
[DEBUG] (2021-08-05 17:02:13.942) VWDictionary.cpp:857::addNewWords() newPts.total()=500
[DEBUG] (2021-08-05 17:02:13.950) VWDictionary.cpp:924::addNewWords() Time to find nn = 0.008231 s
[DEBUG] (2021-08-05 17:02:13.962) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.011213 s
[DEBUG] (2021-08-05 17:02:13.962) VWDictionary.cpp:1061::addNewWords() 113 new words added...
[DEBUG] (2021-08-05 17:02:13.962) VWDictionary.cpp:1063::addNewWords() 387 duplicated words added (from current image = 7)...
[DEBUG] (2021-08-05 17:02:13.962) VWDictionary.cpp:1064::addNewWords() total time 0.019683s
[DEBUG] (2021-08-05 17:02:13.962) Memory.cpp:4742::createSignature() time addNewWords 0.020586s indexed=506 not=113
[DEBUG] (2021-08-05 17:02:13.963) Memory.cpp:5054::createSignature() Bin data kept: rgb=1, depth=1, scan=0, userData=0
[DEBUG] (2021-08-05 17:02:13.996) Memory.cpp:5266::createSignature() time compressing data (id=415) 0.034172s
[DEBUG] (2021-08-05 17:02:13.996) OccupancyGrid.cpp:290::createLocalMap() scan format=NA, occupancyFromDepth_=1 normalsSegmentation_=1 grid3D_=1
[DEBUG] (2021-08-05 17:02:13.996) OccupancyGrid.cpp:367::createLocalMap() Depth image : decimation=4 max=5.000000 min=0.000000
[DEBUG] (2021-08-05 17:02:13.996) util3d.cpp:1051::cloudRGBFromSensorData()
[DEBUG] (2021-08-05 17:02:13.996) util3d.cpp:443::cloudFromDepthRGB()
[DEBUG] (2021-08-05 17:02:13.997) util3d.cpp:554::cloudFromDepthRGB() rgb=1280x720 depth=1280x720 fx=519.366638 fy=519.366638 cx=633.938721 cy=355.219238 (depth factors=1.000000 1.000000) decimation=4
[DEBUG] (2021-08-05 17:02:13.999) util3d.cpp:607::cloudFromDepthRGB()
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:88::segmentCloud() node.getPose()=xyz=-0.010882,0.014730,0.001333 rpy=-0.048645,0.024849,-0.357995 projMapFrame_=0
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:124::segmentCloud() normalKSearch=20
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:125::segmentCloud() maxGroundAngle=0.785398
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:126::segmentCloud() Cluster radius=0.100000
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:127::segmentCloud() flatObstaclesDetected=1
[DEBUG] (2021-08-05 17:02:14.002) OccupancyGrid.hpp:128::segmentCloud() maxGroundHeight=0.000000
[DEBUG] (2021-08-05 17:02:14.055) OccupancyGrid.hpp:142::segmentCloud() viewPoint=0.000000,0.060000,0.000000
[DEBUG] (2021-08-05 17:02:14.055) OccupancyGrid.hpp:164::segmentCloud() groundIndices=483 obstaclesIndices=5486
[DEBUG] (2021-08-05 17:02:14.055) OccupancyGrid.cpp:460::createLocalMap() groundIndices=483, obstaclesIndices=5486
[DEBUG] (2021-08-05 17:02:14.055) OccupancyGrid.cpp:504::createLocalMap() ground=483 obstacles=5486
[DEBUG] (2021-08-05 17:02:14.056) OccupancyGrid.cpp:588::createLocalMap() ground=483 obstacles=5486 empty=0, channels=4
[DEBUG] (2021-08-05 17:02:14.056) SensorData.cpp:407::setOccupancyGrid() ground=483 obstacles=5486 empty=0
[DEBUG] (2021-08-05 17:02:14.060) Memory.cpp:5287::createSignature() time grid map = 0.064265s
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:804::update() time creating signature=180.826904 ms
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:889::addSignatureToStm() adding 415 (pose=xyz=-0.010882,0.014730,0.001333 rpy=-0.048645,0.024849,-0.357995)
[DEBUG] (2021-08-05 17:02:14.061) Signature.cpp:121::addLink() Add link 415 to 191 (type=0 var=0.001000,0.001000)
[DEBUG] (2021-08-05 17:02:14.061) Signature.cpp:121::addLink() Add link 191 to 415 (type=0 var=0.001000,0.001000)
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:970::addSignatureToStm() Min STM id = 1
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:1012::addSignatureToStm() 955 words ref for the signature 415 (weight=0)
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:1020::addSignatureToStm() time = 0.000181s
[DEBUG] (2021-08-05 17:02:14.061) Memory.cpp:3604::rehearsal() Comparing with signature (191)...
[ INFO] (2021-08-05 17:02:14.062) Memory.cpp:3638::rehearsalMerge() old=191, new=415
[ INFO] (2021-08-05 17:02:14.062) Memory.cpp:3658::rehearsalMerge() Rehearsal merging 191 (w=149) and 415 (w=0)
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:3691::rehearsalMerge() fullMerge=true intermediateMerge=false _idUpdatedToNewOneRehearsal=false
[DEBUG] (2021-08-05 17:02:14.062) Signature.cpp:202::removeLink() Removed link 415 from 191
[DEBUG] (2021-08-05 17:02:14.062) Signature.cpp:202::removeLink() Removed link 191 from 415
[DEBUG] (2021-08-05 17:02:14.062) Signature.cpp:121::addLink() Add link 191 to 415 (type=1 var=1.000000,1.000000)
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:3757::rehearsalMerge() New weights: 191->150 415->150
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:2320::moveToTrash() id=415
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:3383::removeVirtualLinks()
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:5406::disableWordsRef() id=415
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:5422::disableWordsRef() 500 words total ref removed from signature 415... (total active ref = 1000)
[DEBUG] (2021-08-05 17:02:14.062) DBDriver.cpp:386::asyncSave() s=415
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:3627::rehearsal() merged=191, sim=0.660000 t=0.001284s
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:825::update() time rehearsal=1.555920 ms
[DEBUG] (2021-08-05 17:02:14.062) Memory.cpp:878::update() totalTimer = 0.182669s
[DEBUG] (2021-08-05 17:02:14.063) Memory.cpp:2478::getLastWorkingSignature()
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1236::process() Processing signature 191 w=150 map=0
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1238::process() timeMemoryUpdate=0.182873s
[DEBUG] (2021-08-05 17:02:14.063) Rtabmap.cpp:1414::process() Added pose xyz=-0.013611,0.012732,-0.000638 rpy=-0.015495,-0.030264,-0.311681 (odom=xyz=-0.013611,0.012732,-0.000638 rpy=-0.015495,-0.030264,-0.311681)
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1603::process() timeProximityByTimeDetection=0.000123s
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1622::process() computing likelihood...
[DEBUG] (2021-08-05 17:02:14.063) Memory.cpp:1870::computeLikelihood() processing...
[DEBUG] (2021-08-05 17:02:14.063) Memory.cpp:1907::computeLikelihood() compute likelihood (tf-idf) 0.000162 s
[DEBUG] (2021-08-05 17:02:14.063) Rtabmap.cpp:4349::adjustLikelihood() likelihood.size()=1
[DEBUG] (2021-08-05 17:02:14.063) Rtabmap.cpp:4368::adjustLikelihood() values.size=0
[DEBUG] (2021-08-05 17:02:14.063) Rtabmap.cpp:4415::adjustLikelihood() mean=0.000000, stdDev=0.000000, max=0.000000, maxId=0, time=0.000021s
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1740::process() timeLikelihoodCalculation=0.000434s
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1746::process() getting posterior...
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:147::computePosterior()
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:179::computePosterior() STEP1-generate prior=0.000002s, rows=1, cols=1
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:714::updatePosterior()
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:191::computePosterior() STEP1-update posterior=0.000000s, posterior=1, _posterior size=1
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:197::computePosterior() STEP1-matrix mult time=0.000090s
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:200::computePosterior() STEP1-matrix mult time=0.000022s
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:219::computePosterior() STEP2-likelihood time=0.000021s
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:223::computePosterior() sum=2.000000
[DEBUG] (2021-08-05 17:02:14.063) BayesFilter.cpp:231::computePosterior() normalize time=0.000038s
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1751::process() timePosteriorCalculation=0.000251s
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1762::process() creating hypotheses...
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1776::process() Highest hypothesis=0, value=0.000000, timeHypothesesCreation=0.000039s
[DEBUG] (2021-08-05 17:02:14.063) Memory.cpp:2150::joinTrashThread()
[DEBUG] (2021-08-05 17:02:14.063) Memory.cpp:2152::joinTrashThread()
[ INFO] (2021-08-05 17:02:14.063) Rtabmap.cpp:1851::process() Time emptying memory trash = 0.025365s,  joining (actual overhead) = 0.000057s
[DEBUG] (2021-08-05 17:02:14.064) Graph.cpp:2174::getNodesInRadius() found nodes=1
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:2143::process() near nodes=1, max local immunized=0, ratio=0.250000 WM=1
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:2229::process() timeReactivations=0.000238s
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2307::process() Proximity detection (local loop closure in SPACE using matching images, local radius=10.000000m)
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2317::process() nearestIds=2/2
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2326::process() nearestPoses=0
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2330::process() got 0 paths
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2349::process() nearestPaths=0 proximityMaxPaths=3
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:2448::process() timeProximityBySpaceVisualDetection=0.000176s
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2453::process() Proximity detection (local loop closure in SPACE with scan matching)
[DEBUG] (2021-08-05 17:02:14.064) Rtabmap.cpp:2456::process() Proximity by scan matching is disabled (RGBD/ProximityPathMaxNeighbors=0).
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:2603::process() timeProximityBySpaceDetection=0.000080s
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:2667::process() timeAddLoopClosureLink=0.000019s
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3158::process() timeMapOptimization=0.000026s
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3185::process() sending stats...
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3197::process() send all stats...
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3318::process() Localization pose = xyz=-0.013611,0.012732,-0.000638 rpy=-0.015495,-0.030264,-0.311681
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3322::process() Set map correction = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] (2021-08-05 17:02:14.064) Rtabmap.cpp:3395::process() Time creating stats = 0.000426...
[DEBUG] (2021-08-05 17:02:14.065) Memory.cpp:2686::removeRawData() id=191 image=1 scan=0 userData=1
[DEBUG] (2021-08-05 17:02:14.065) Memory.cpp:2009::cleanup()
[DEBUG] (2021-08-05 17:02:14.065) Memory.cpp:2604::saveLocationData() Saving location data 191
[ INFO] (2021-08-05 17:02:14.065) Rtabmap.cpp:3475::process() timeMemoryCleanup = 0.000391s... 0 signatures removed
[ INFO] (2021-08-05 17:02:14.065) Rtabmap.cpp:3488::process() Total time processing = 0.185171s...
[ INFO] (2021-08-05 17:02:14.065) Rtabmap.cpp:3618::process() Time limit reached processing = 0.000060...
[DEBUG] (2021-08-05 17:02:14.065) Rtabmap.cpp:3659::process()
[ INFO] (2021-08-05 17:02:14.065) Rtabmap.cpp:3662::process() Adding data 191 [0] (rgb/left=0 depth/right=0)
[DEBUG] (2021-08-05 17:02:14.065) Rtabmap.cpp:3685::process()
[DEBUG] (2021-08-05 17:02:14.066) Rtabmap.cpp:3754::process() wmState=2
[DEBUG] (2021-08-05 17:02:14.066) DBDriverSqlite3.cpp:4648::addStatisticsQuery() Ref ID = 415
[DEBUG] (2021-08-05 17:02:14.067) DBDriverSqlite3.cpp:4721::addStatisticsQuery() Time=0.001339s
[DEBUG] (2021-08-05 17:02:14.067) Rtabmap.cpp:3764::process() Empty trash...
[DEBUG] (2021-08-05 17:02:14.067) DBDriver.cpp:315::emptyTrashes() Async emptying, start the trash thread
[DEBUG] (2021-08-05 17:02:14.068) Rtabmap.cpp:3841::process() End process, timeFinalizingStatistics=0.002619s
[DEBUG] (2021-08-05 17:02:14.068) CoreWrapper.cpp:3786::publishStats() Publishing stats...
[DEBUG] (2021-08-05 17:02:14.068) MapsManager.cpp:479::updateMapCaches() Updating map caches...
[DEBUG] (2021-08-05 17:02:14.068) OctoMap.cpp:384::update() Update (poses=2 addedNodes_=2)
[DEBUG] (2021-08-05 17:02:14.068) OctoMap.cpp:529::update() Last id = 191
[DEBUG] (2021-08-05 17:02:14.068) OctoMap.cpp:546::update() orderedPoses = 0


Kindly regards and i really appreciate your help :)
bruchpilot123
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Icp Odometry

bruchpilot123
I just uploaded a rosbag, during recording the rosbag buffer exceeded. That might be the reason, that the rgb data are worse. But at least the lidar data are okay... Link

Kindly regards,

Robert Scheffler
matlabbe
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Icp Odometry

matlabbe
Administrator
Hi,

The frame rate of the lidar in the bag is quite slow (5 Hz) while not having a very large field of view (like other 360 lidars), so icp_odometry will not likely work unless the robot is moving very slowly.

The zed image data seem not correctly recorded too, so I only tested a setup with zed odom + lidar: 
roslaunch rtabmap_ros rtabmap.launch \
   visual_odometry:=false \
   subscribe_scan_cloud:=true \
   scan_cloud_topic:=/bf_lidar/points_raw \
   odom_topic:=/zed2/zed_node/odom \
   frame_id:=base_link \
   use_sim_time:=true \
   subscribe_rgb:=false \
   depth:=false  \
   approx_sync:=true \
   queue_size:=20 \
   args:="-d"

rosbag play --clock 2021-08-06-12-44-19.bag



Looking at the trajectory, the zed odometry seems okay if you indeed landed near the takeoff place. The lidar is wrongly assembled, so if odometry is good, the problem is coming from the static TF transform between base_link and the lidar link (TF between zed camera and lidar is wrong).

Another issue I saw about the lidar scans, sometimes they have double surface in a single scan, for example at 55 sec in the bag (note sure if there is a problem with the lidar config, or a lidar skewing problem):


Note also when you share large bags over internet, compress them before sharing them: 
rosbag compress 2021-08-06-12-44-19.bag

cheers,
Mathieu
Free forum by Nabble Edit this page