Zed RTAB-Map Windows

[ahsteven]

I am trying to use the Zed camera with RTAB-Map on Windows. First I am trying to make it work using the Gui then I will use the RGBD cpp example.

I have Zed sdk 2.0 installed. When I go to Detection>select source>stereo camera>zed camera, zed sdk is greyed out and the only option is Stereo Usb camera which inputs my computers webcam not the zed camera.

Any tips on running the Zed with the rtabmap Gui? Will Zed sdk 2.0 work with RTAB-Map? I know some of the function names slightly changed from the 1.2 version. Thanks

[matlabbe]

Hi,

which RTAB-Map version is installed? If you build RTAB-Map from source, make sure cmake is detecting ZED sdk (see WITH_ZED=YES at the end of cmake status), otherwise the ZED sdk driver will be disabled.

To use stereo usb camera input source with the correct camera, open Preferences->Source and change "ID of the device" to 1 or 2. You will need to do a manual stereo calibration with a checkboard, unlike with ZED SDK driver.

If you install the binaries, make sure it is the version with CUDA and ZED: https://github.com/introlab/rtabmap/releases

cheers,
Mathieu

[ahsteven]

Hello Mathieu,
Thank you for your response. I updated to RTAB-Map .13 with Zed 20 and Cuda 8 support. So then I used the calibration tool to generate the calibration files for my Zed camera.

After that I took some video around my garage. It seems to do fine until a loop closure. At which point I believe it makes a false detection and puts me somewhere slightly off then it is unable to recover. It could be something else but both times I ran it it lost tracking after I returned to my stating point and was unable to relocalize.

Any reason why this might be happening. Is there a parameter I can set to make false loop closures less likely? I don't know if thats the issue and I don't recall having this issue when I ran it on Linux some time ago. I have included a view of the graph so you can see where it stops.

[matlabbe]

Hi,

Do you have a sample database to look at? For the lost odometry problem, use Data recorder tool (see Tools->Data Recorder...) to save all images even if odomerty is lost. You can decrease source frame rate to 10/15 Hz to decrease output file size.

cheers,
Mathieu

[ahsteven]

Thank you Mathieu,

I removed the calibration file that I had made using the RTABMAP tool. I read on a post that if you use this it takes in the depth with the CPU as opposed to the GPU. I left it blank so that it used the ZED calibration file with the GPU. Does this sound right?The performance was good when I made this correction. I had to reset the odometry from time to time but it was able to merge the maps together which will work fine for me.

I will be switching over to c++ code now and I was wondering if you have any suggestions for a variable(s) to monitor to trigger an odometry reset? I will have other sensors providing odometry information namely an imu and wheel odometry. I wanted to use RTABMAP to give me a global location when I turn on the robot and to reset the drift of the other two sensors from time to time. That being said, a false loop closure would be very harmful to the performance. I was planning to pass in a Kalman filtered odometry of the imu, wheel odometry and Rtabmap odometry into RTABMAP. The RTABMAP odometry is the only one capable of a global odometry so I am not sure the best way to handle the confidence of this measurement when input into the Kalman filter. Do you have any suggestions for this approach and to better ensure there is not a false loop closure?

Also, I have attached a screenshot of my current settings and I was wondering if you could confirm that these are set correctly to achieve the best performance with Zed.



Thank you so much for all your help.

-Adam

[matlabbe]

Hi,

The calibration is if you don't use the ZED SDK driver (i.e., the usb side-by-side driver). With ZED SDK, the calibration is taken from the camera and the depth is computed on GPU.

There is a parameter "Odom/ResetCountdown" that can be used to automatically reset odometry:


In the code, when lost, the published pose here is null.

A good way to reject almost all wrong loop closures is to decrease this parameter "RGBD/OptimizeMaxError" (to 0.1 for example, too low it will reject all loop closures):


However, before you start modifying the code under Windows, I may suggest to move on Ubuntu/ROS (with rtabmap_ros package). The sensor fusion you are referring looks like very much to robot_localization package, which is an EKF.

cheers,
Mathieu

[ahsteven]

Thank you Mathieu. Your comments are most helpful. I look forward to seeing
the results after tuning these parameters.

Yes, I would like to move to ROS in the future. However, the robot I am
working with has all of its code working under windows currently. I am
still getting familiar with Linux and ROS so it would take me some time to
get everything set up over there. I have implemented an EKF and UKF in
windows for a different project so I think I will be able to manage that on
Windows. I will definitely refer to the robot_localization code as a
reference. Thank you for that suggestion. Are there any other pitfalls I
will run into on Windows?

[matlabbe]

Hi,

The only thing is that on ROS you can have outputs in standard ROS messages (e.g., map, point cloud, pose...) for other external applications (or nodes in ROS terms). On Windows (the standalone application), there are no outputs outside the application (no TCP/IP interface implemented). Hard-coding stuff inside the app is then required, making less upgradable when rtabmap code is updated on github.

Sensor fusion of IMU/wheel odometry may be added to Odometry or OdometryThread class. To get map outputs, maybe the easier is to do it in the GUI at the end of MainWindow::processStatistics(), where all data from the mapping thread have been processed and assembled.

cheers,
Mathieu

[ahsteven]

Hi Mathieu,

I have been testing different parameters in the Gui to see what produces the best results. I have found that automatically resetting odometry after 1 consecutive image which odometry cannot be computed produces pretty good results. Also using the Kalman filtering pose estimation seems to take away some skew error in the graph. I am currenly using the vertigo for robust graph optimization and improved loop closure detection. I will continue to tune parameters but the results are pretty good so far.

My goal now is to import these settings into the RGBD-Mapping c++ example. I have the saved config.ini file from the gui. Imported in to the RGBD example as follows:

        Rtabmap * rtabmap = new Rtabmap();
        rtabmap->init("config.ini","test_10_I.db");

I also set the optical transform as:
       Transform opticalRotation(0,0,-1,0, -1,0,0,0, 0,1,0,0);

Since Zed is  x axis to the right, y axis is up and z axis is backwards.

The gui for the c++ code does not change size so I am unable to see where the odometry is. So I take the saved db file and import back into the gui.

When I view the db file with the RTABMAP gui I see the odometry starts working after a loop closure. For my test I walk around my garage twice. When I record the db file with the c++ code I see the odometry stops computing after one lap. However, when I perform the same test with the gui the results are as I expect.

Here are pictures of the correct graphs as produced by the gui.





Here are some images of the incorrect graphs of seperate test runs produced from the db file recorded with the c++ code and then displayed in the gui. The odometry seems to stop when I get back to the starting point.





Do you have an idea of what is happening here?

Here are my config settings:

[Camera]
imgRate = 20
mirroring = false
calibrationName =
type = 1
device =
localTransform = 0 0 0 -PI_2 0 -PI_2
imageDecimation = 1
rgbd\driver = 1
rgbd\rgbdColorOnly = false
rgbd\distortion_model =
rgbd\bilateral = false
rgbd\bilateral_sigma_s = 10
rgbd\bilateral_sigma_r = 0.10000000000000001
stereo\driver = 4
stereo\depthGenerated = false
rgb\driver = 0
rgb\rectify = false
Openni\oniPath =
Openni2\autoWhiteBalance = true
Openni2\autoExposure = true
Openni2\exposure = 0
Openni2\gain = 100
Openni2\mirroring = false
Openni2\stampsIdsUsed = false
Openni2\oniPath =
Freenect2\format = 0
Freenect2\minDepth = 0.29999999999999999
Freenect2\maxDepth = 12
Freenect2\bilateralFiltering = true
Freenect2\edgeAwareFiltering = true
Freenect2\noiseFiltering = true
RealSense\presetRGB = 0
RealSense\presetDepth = 2
RealSense\odom = false
RGBDImages\path_rgb =
RGBDImages\path_depth =
RGBDImages\scale = 1
StereoImages\path_left = C:/Users/Adam/Documents/stereo_20Hz/stereo_20Hz/left
StereoImages\path_right = C:/Users/Adam/Documents/stereo_20Hz/stereo_20Hz/right
StereoImages\rectify = false
StereoVideo\path = C:/Users/Adam/Documents/RTAB-Map/zed_cal2.avi
StereoVideo\path2 =
StereoZed\resolution = 2
StereoZed\quality = 1
StereoZed\self_calibration = true
StereoZed\sensing_mode = 1
StereoZed\confidence_thr = 98
StereoZed\odom = false
StereoZed\svo_path =
Images\path =
Images\startPos = 1
Images\refreshDir = false
Images\bayerMode = 0
Images\filenames_as_stamps = false
Images\sync_stamps = true
Images\stamps =
Images\path_scans =
Images\scan_transform = 0 0 0 0 0 0
Images\scan_max_pts = 0
Images\scan_downsample_step = 1
Images\scan_voxel_size = 0.025000000000000001
Images\odom_path =
Images\odom_format = 0
Images\gt_path =
Images\gt_format = 0
Video\path =
ScanFromDepth\enabled = false
ScanFromDepth\decimation = 8
ScanFromDepth\maxDepth = 4
ScanFromDepth\voxelSize = 0.025000000000000001
ScanFromDepth\normalsK = 20
DepthFromScan\depthFromScan = false
DepthFromScan\depthFromScanFillHoles = true
DepthFromScan\depthFromScanVertical = true
DepthFromScan\depthFromScanHorizontal = false
DepthFromScan\depthFromScanFillBorders = false
Database\path = /home/mathieu/Documents/RTAB-Map/map1_8Hz.db
Database\ignoreOdometry = false
Database\ignoreGoalDelay = false
Database\ignoreGoals = true
Database\startPos = 0
Database\cameraIndex = -1
Database\useDatabaseStamps = false


[Gui]
General\imagesKept = true
General\cloudsKept = true
General\loggerLevel = 2
General\loggerEventLevel = 3
General\loggerPauseLevel = 3
General\loggerType = 1
General\loggerPrintTime = true
General\loggerPrintThreadId = false
General\verticalLayoutUsed = true
General\imageRejectedShown = true
General\imageHighestHypShown = false
General\beep = false
General\figure_time = true
General\figure_cache = true
General\notifyNewGlobalPath = true
General\odomQualityThr = 50
General\odomOnlyInliersShown = false
General\posteriorGraphView = true
General\odomDisabled = false
General\odomRegistration = 3
General\gtAlign = true
General\showClouds0 = true
General\decimation0 = 4
General\maxDepth0 = 4
General\minDepth0 = 0
General\roiRatios0 = 0.0 0.0 0.0 0.0
General\showScans0 = true
General\showFeatures0 = false
General\downsamplingScan0 = 1
General\voxelSizeScan0 = 0
General\opacity0 = 1
General\ptSize0 = 1
General\opacityScan0 = 1
General\ptSizeScan0 = 1
General\ptSizeFeatures0 = 3
General\showClouds1 = true
General\decimation1 = 2
General\maxDepth1 = 0
General\minDepth1 = 0
General\roiRatios1 = 0.0 0.0 0.0 0.0
General\showScans1 = true
General\showFeatures1 = true
General\downsamplingScan1 = 1
General\voxelSizeScan1 = 0
General\opacity1 = 1
General\ptSize1 = 2
General\opacityScan1 = 1
General\ptSizeScan1 = 1
General\ptSizeFeatures1 = 1
General\cloudVoxel = 0
General\cloudNoiseRadius = 0
General\cloudNoiseMinNeighbors = 5
General\cloudCeilingHeight = 0
General\cloudFloorHeight = 0
General\normalKSearch = 10
General\scanCeilingHeight = 0
General\scanFloorHeight = 0
General\scanNormalKSearch = 0
General\showGraphs = true
General\showFrustums = false
General\showLabels = false
General\noFiltering = false
General\cloudFiltering = true
General\cloudFilteringRadius = 0.10000000000000001
General\cloudFilteringAngle = 30
General\subtractFiltering = false
General\subtractFilteringMinPts = 5
General\subtractFilteringRadius = 0.02
General\subtractFilteringAngle = 0
General\gridMapShown = false
General\gridMapOpacity = 0.75
General\octomap = false
General\octomap_depth = 16
General\octomap_full_update = false
General\octomap_2dgrid = true
General\octomap_3dmap = true
General\octomap_cube = true
General\octomap_occupancy_thr = 0.5
General\octomap_point_size = 5
General\meshing = false
General\meshing_angle = 15
General\meshing_quad = true
General\meshing_texture = false
General\meshing_triangle_size = 2


[CalibrationDialog]
board_width = 9
board_height = 6
board_square_size = 0.033799999999999997
max_scale = 1
geometry = @ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\x18\0\0\x5!\0\0\x3\x32\0\0\0\0\0\0\0\x18\0\0\x5!\0\0\x3\x32\0\0\0\0\0\0\0\0\a\x80)


[Core]
Version = 0.13.0
BRIEF\Bytes = 32
BRISK\Octaves = 3
BRISK\PatternScale = 1
BRISK\Thresh = 30
Bayes\FullPredictionUpdate = false
Bayes\PredictionLC = 0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23
Bayes\VirtualPlacePriorThr = 0.9
DbSqlite3\CacheSize = 10000
DbSqlite3\InMemory = false
DbSqlite3\JournalMode = 3
DbSqlite3\Synchronous = 0
DbSqlite3\TempStore = 2
FAST\Gpu = false
FAST\GpuKeypointsRatio = 0.05
FAST\GridCols = 4
FAST\GridRows = 4
FAST\MaxThreshold = 200
FAST\MinThreshold = 1
FAST\NonmaxSuppression = true
FAST\Threshold = 30
FREAK\NOctaves = 4
FREAK\OrientationNormalized = true
FREAK\PatternScale = 22
FREAK\ScaleNormalized = true
GFTT\BlockSize = 3
GFTT\K = 0.04
GFTT\MinDistance = 5
GFTT\QualityLevel = 0.01
GFTT\UseHarrisDetector = false
GTSAM\Optimizer = 1
Grid\3D = true
Grid\CellSize = 0.05
Grid\ClusterRadius = 0.1
Grid\DepthDecimation = 4
Grid\DepthMax = 4.1
Grid\DepthMin = 0.0
Grid\DepthRoiRatios = 0.0 0.0 0.0 0.0
Grid\FlatObstacleDetected = true
Grid\FootprintHeight = 0.0
Grid\FootprintLength = 0.0
Grid\FootprintWidth = 0.0
Grid\FromDepth = true
Grid\GroundIsObstacle = true
Grid\MapFrameProjection = false
Grid\MaxGroundAngle = 45
Grid\MaxGroundHeight = 0.0
Grid\MaxObstacleHeight = 0.0
Grid\MinClusterSize = 10
Grid\MinGroundHeight = 0
Grid\NoiseFilteringMinNeighbors = 5
Grid\NoiseFilteringRadius = 0.0
Grid\NormalK = 10
Grid\NormalsSegmentation = true
Grid\ProjRayTracing = false
Grid\Scan2dMaxFilledRange = 4
Grid\Scan2dUnknownSpaceFilled = false
Grid\ScanDecimation = 1
GridGlobal\Eroded = false
GridGlobal\FootprintRadius = 0.0
GridGlobal\FullUpdate = true
GridGlobal\MinSize = 0.0
Icp\CorrespondenceRatio = 0.7
Icp\DownsamplingStep = 1
Icp\Epsilon = 0
Icp\Iterations = 30
Icp\MaxCorrespondenceDistance = 0.1
Icp\MaxRotation = 0.78
Icp\MaxTranslation = 0.2
Icp\PointToPlane = false
Icp\PointToPlaneNormalNeighbors = 20
Icp\VoxelSize = 0.005
Kp\BadSignRatio = 0.2
Kp\DetectorStrategy = 0
Kp\DictionaryPath =
Kp\IncrementalDictionary = true
Kp\IncrementalFlann = true
Kp\MaxDepth = 0
Kp\MaxFeatures = 400
Kp\MinDepth = 0
Kp\NNStrategy = 1
Kp\NewWordsComparedTogether = true
Kp\NndrRatio = 0.8
Kp\Parallelized = true
Kp\RoiRatios = 0.0 0.0 0.0 0.0
Kp\SubPixEps = 0.02
Kp\SubPixIterations = 0
Kp\SubPixWinSize = 3
Kp\TfIdfLikelihoodUsed = true
Mem\BadSignaturesIgnored = false
Mem\BinDataKept = true
Mem\CompressionParallelized = true
Mem\GenerateIds = true
Mem\ImageKept = true
Mem\ImagePostDecimation = 1
Mem\ImagePreDecimation = 1
Mem\IncrementalMemory = true
Mem\InitWMWithAllNodes = false
Mem\IntermediateNodeDataKept = false
Mem\LaserScanDownsampleStepSize = 1
Mem\LaserScanNormalK = 0
Mem\MapLabelsAdded = true
Mem\NotLinkedNodesKept = true
Mem\RawDescriptorsKept = true
Mem\RecentWmRatio = 0.2
Mem\ReduceGraph = false
Mem\RehearsalIdUpdatedToNewOne = false
Mem\RehearsalSimilarity = 0.6
Mem\RehearsalWeightIgnoredWhileMoving = false
Mem\STMSize = 10
Mem\SaveDepth16Format = false
Mem\TransferSortingByWeightId = false
Mem\UseOdomFeatures = false
ORB\EdgeThreshold = 31
ORB\FirstLevel = 0
ORB\Gpu = false
ORB\NLevels = 1
ORB\PatchSize = 31
ORB\ScaleFactor = 1.2
ORB\ScoreType = 0
ORB\WTA_K = 2
Odom\AlignWithGround = false
Odom\FillInfoData = true
Odom\FilteringStrategy = 1
Odom\GuessMotion = false
Odom\Holonomic = false
Odom\ImageBufferSize = 1
Odom\ImageDecimation = 1
Odom\KalmanMeasurementNoise = 0.01
Odom\KalmanProcessNoise = 0.001
Odom\KeyFrameThr = 0.3
Odom\ParticleLambdaR = 100
Odom\ParticleLambdaT = 100
Odom\ParticleNoiseR = 0.002
Odom\ParticleNoiseT = 0.002
Odom\ParticleSize = 400
Odom\ResetCountdown = 1
Odom\ScanKeyFrameThr = 0.7
Odom\Strategy = 0
Odom\VisKeyFrameThr = 100
OdomF2M\BundleAdjustment = 0
OdomF2M\BundleAdjustmentMaxFrames = 0
OdomF2M\MaxNewFeatures = 0
OdomF2M\MaxSize = 1000
OdomF2M\ScanMaxSize = 2000
OdomF2M\ScanSubtractRadius = 0.05
OdomFovis\BucketHeight = 80
OdomFovis\BucketWidth = 80
OdomFovis\CliqueInlierThreshold = 0.1
OdomFovis\FastThreshold = 20
OdomFovis\FastThresholdAdaptiveGain = 0.005
OdomFovis\FeatureSearchWindow = 25
OdomFovis\FeatureWindowSize = 9
OdomFovis\InlierMaxReprojectionError = 1.5
OdomFovis\MaxKeypointsPerBucket = 25
OdomFovis\MaxMeanReprojectionError = 10.0
OdomFovis\MaxPyramidLevel = 3
OdomFovis\MinFeaturesForEstimate = 10
OdomFovis\MinPyramidLevel = 0
OdomFovis\StereoMaxDisparity = 128
OdomFovis\StereoMaxDistEpipolarLine = 1.5
OdomFovis\StereoMaxRefinementDisplacement = 1.0
OdomFovis\StereoRequireMutualMatch = true
OdomFovis\TargetPixelsPerFeature = 250
OdomFovis\UpdateTargetFeaturesWithRefined = false
OdomFovis\UseAdaptiveThreshold = true
OdomFovis\UseBucketing = true
OdomFovis\UseHomographyInitialization = true
OdomFovis\UseImageNormalization = false
OdomFovis\UseSubpixelRefinement = true
OdomMono\InitMinFlow = 100
OdomMono\InitMinTranslation = 0.1
OdomMono\MaxVariance = 0.01
OdomMono\MinTranslation = 0.02
OdomORBSLAM2\Bf = 0.076
OdomORBSLAM2\ThDepth = 41
OdomORBSLAM2\VocPath =
OdomViso2\BucketHeight = 50
OdomViso2\BucketMaxFeatures = 2
OdomViso2\BucketWidth = 50
OdomViso2\InlierThreshold = 2.0
OdomViso2\MatchBinsize = 50
OdomViso2\MatchDispTolerance = 2
OdomViso2\MatchHalfResolution = true
OdomViso2\MatchMultiStage = true
OdomViso2\MatchNmsN = 3
OdomViso2\MatchNmsTau = 50
OdomViso2\MatchOutlierDispTolerance = 5
OdomViso2\MatchOutlierFlowTolerance = 5
OdomViso2\MatchRadius = 200
OdomViso2\MatchRefinement = 1
OdomViso2\RansacIters = 200
OdomViso2\Reweighting = true
Optimizer\Epsilon = 0.001
Optimizer\Iterations = 100
Optimizer\Robust = true
Optimizer\Strategy = 0
Optimizer\VarianceIgnored = false
RGBD\AngularUpdate = 0
RGBD\CreateOccupancyGrid = true
RGBD\Enabled = true
RGBD\GoalReachedRadius = 0.5
RGBD\GoalsSavedInUserData = false
RGBD\LinearUpdate = 0
RGBD\LocalImmunizationRatio = 0.25
RGBD\LocalRadius = 15
RGBD\LoopClosureReextractFeatures = true
RGBD\MaxLocalRetrieved = 2
RGBD\NeighborLinkRefining = false
RGBD\NewMapOdomChangeDistance = 0
RGBD\OptimizeFromGraphEnd = false
RGBD\OptimizeMaxError = 0
RGBD\PlanAngularVelocity = 0
RGBD\PlanLinearVelocity = 0
RGBD\PlanStuckIterations = 0
RGBD\ProximityAngle = 45
RGBD\ProximityBySpace = false
RGBD\ProximityByTime = false
RGBD\ProximityMaxGraphDepth = 0
RGBD\ProximityMaxPaths = 3
RGBD\ProximityPathFilteringRadius = 0.5
RGBD\ProximityPathMaxNeighbors = 10
RGBD\ProximityPathRawPosesUsed = true
RGBD\ScanMatchingIdsSavedInLinks = true
Reg\Force3DoF = false
Reg\Strategy = 0
Reg\VarianceFromInliersCount = false
Reg\VarianceNormalized = false
Rtabmap\CreateIntermediateNodes = false
Rtabmap\DetectionRate = 1
Rtabmap\ImageBufferSize = 1
Rtabmap\LoopRatio = 0
Rtabmap\LoopThr = 0
Rtabmap\MaxRetrieved = 2
Rtabmap\MemoryThr = 0
Rtabmap\PublishLastSignature = true
Rtabmap\PublishLikelihood = true
Rtabmap\PublishPdf = true
Rtabmap\PublishStats = true
Rtabmap\StartNewMapOnLoopClosure = false
Rtabmap\StatisticLogged = false
Rtabmap\StatisticLoggedHeaders = true
Rtabmap\StatisticLogsBufferedInRAM = true
Rtabmap\TimeThr = 0
Rtabmap\VhStrategy = 0
Rtabmap\WorkingDirectory = C:/Users/Adam/Documents/RTAB-Map
SIFT\ContrastThreshold = 0.04
SIFT\EdgeThreshold = 10
SIFT\NFeatures = 0
SIFT\NOctaveLayers = 3
SIFT\Sigma = 1.6
SURF\Extended = false
SURF\GpuKeypointsRatio = 0.01
SURF\GpuVersion = false
SURF\HessianThreshold = 150
SURF\OctaveLayers = 2
SURF\Octaves = 4
SURF\Upright = false
Stereo\Eps = 0.01
Stereo\Iterations = 30
Stereo\MaxDisparity = 128
Stereo\MaxLevel = 3
Stereo\MinDisparity = 1
Stereo\OpticalFlow = true
Stereo\SSD = true
Stereo\WinHeight = 3
Stereo\WinWidth = 16
StereoBM\BlockSize = 15
StereoBM\MinDisparity = 0
StereoBM\NumDisparities = 128
StereoBM\PreFilterCap = 31
StereoBM\PreFilterSize = 9
StereoBM\SpeckleRange = 4
StereoBM\SpeckleWindowSize = 100
StereoBM\TextureThreshold = 10
StereoBM\UniquenessRatio = 15
VhEp\MatchCountMin = 8
VhEp\RansacParam1 = 3
VhEp\RansacParam2 = 0.99
Vis\BundleAdjustment = 0
Vis\CorFlowEps = 0.01
Vis\CorFlowIterations = 30
Vis\CorFlowMaxLevel = 3
Vis\CorFlowWinSize = 16
Vis\CorGuessWinSize = 20
Vis\CorNNDR = 0.9
Vis\CorNNType = 3
Vis\CorType = 0
Vis\EpipolarGeometryVar = 0.02
Vis\EstimationType = 0
Vis\FeatureType = 6
Vis\ForwardEstOnly = true
Vis\InlierDistance = 0.1
Vis\Iterations = 30
Vis\MaxDepth = 10
Vis\MaxFeatures = 0
Vis\MinDepth = 0
Vis\MinInliers = 10
Vis\PnPFlags = 1
Vis\PnPRefineIterations = 1
Vis\PnPReprojError = 5
Vis\RefineIterations = 5
Vis\RoiRatios = 0.03 0.03 0.04 0.04
Vis\SubPixEps = 0.02
Vis\SubPixIterations = 0
Vis\SubPixWinSize = 3
g2o\Baseline = 0.075
g2o\Optimizer = 0
g2o\PixelVariance = 1main.cpp
g2o\RobustKernelDelta = 8
g2o\Solver = 0


I have uploaded my main.cpp file as an attachment.

Thank you for your help.

[matlabbe]

Hi,

The difference may be that Odometry parameters are not set with those in the config.ini:

OdometryThread odomThread(new OdometryF2M());

You could try this instead:

ParametersMap parameters;
Parameters::readINI("config.ini", parameters);
OdometryThread odomThread(Odometry::create(parameters));

cheers,
Mathieu

[ahsteven]

Hi Mathieu,

Thank you for the suggestion. It works great now.

My next step will be to test with a source build of RTABMAP and then to modify the odometry thread as you suggested in your previous post.

Thank you for your help.

-Adam

[ahsteven]

Hi Mathieu,

It seems I am not getting the same performance from my source build as I was getting from the precompiled binaries particularly when I run the RGBD example. That being said I think I can accomplish everything I want to accomplish without modifying the rtabmap source code and I would like to confirm. What I would like to do is as follows:

1. Initialize the rtabmap coordinates at startup
2. Receive a flag indicating if there are not enough features or tracking has been lost
3. -reset the odometry using an external odometry source if tracking lost
4. Get a flag when after odometry statistics and optimization has been processed
5. -receive pose

Is it possible to do this without modifying the source code? Would you be so kind as to clarify which functions I would call to do each of these 5 things?

Thank you for your help.

[matlabbe]

Hi,

Depending on the binary version you used, the default parameters may be not the same using latest version from source.

I updated the source code to be able to reset odometry with a pose (in similar way we can do in ROS). From the C++ RGBD example:

1- Call this before starting the camera but after odomThread.registerToEventsManager();

UEventsManager::post(new OdometryResetEvent(Transform(x,y,z,roll,pitch,yaw)));

2- See how the odometry events are handled in MapBuilder. When pose is null, odometry is lost. The OdometryEvent has info member that have more information about status of odometry (e.g., inliers, features extracted...).

3- That could be done in the processOdometry callback of MapBuilder, calling again:

UEventsManager::post(new OdometryResetEvent(Transform(x,y,z,roll,pitch,yaw)));

4- The Odometry Event is sent after odometry has been uptated. The Statistics object is sent through a RtabmapEvent after the map has been updated (after optimization): see processStatistics().

5- Current optimized pose is the last in Statistics::getPoses().

cheers,
Mathieu

[ahsteven]

Thank you for such a great response and updating the code. I will implement these suggestions.

[ahsteven]

Hello Mathieu,

I updated the RTABMap code to the new commit however when I close the viewer I am getting an exception.

With 13.0 I was not getting the exception. The code is the same. I have tried using the sample stereo images as well as feeding the Zed directly. I get the same exception after closing the viewing window.

Here is the error:

Saving rtabmap_cloud.pcd...
Voxel grid filtering of the assembled cloud (voxel=0.010000, 57600 points)
[FATAL] (2017-08-16 11:47:33) util3d_filtering.cpp:184::rtabmap::util3d::voxelize() Condition ((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())) not met! [Cloud size=57600 indices=0 is_dense=false]

Is this caused by something in the new commit?

-Adam

[matlabbe]

Hi,

yes, in recent versions there is an assert checking if the cloud to voxelize is not empty if the cloud is dense or indices are not empty if cloud is organized (to avoid crash in PCL).

In your log, the indices vector is empty and the cloud is not dense, which is causing the error. However, the voxelized cloud should not be organized at this point, as pcl::removeNaNFromPointCloud() is called before adding points to output cloud. Normally, pcl::removeNaNFromPointCloud() should convert any point cloud to dense. Which PCL version are you using?

cheers,
Mathieu

[ahsteven]

Hi Mathieu,

I am using PCL 1.8.0

Best,

Adam

[matlabbe]

Strange, in PCL 1.8.0, the cloud should be set to dense: https://github.com/PointCloudLibrary/pcl/blob/pcl-1.8.0/filters/include/pcl/filters/impl/filter.hpp#L91

On my side, I cannot reproduce the problem. Well, you may comment the voxelize() part of the example.

To debug this, check if the cloud generated by pcl::removeNaNFromPointCloud is outputting a dense cloud:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmpNoNaN(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<int> index;
pcl::removeNaNFromPointCloud(*tmp, *tmpNoNaN, index);
if(!tmpNoNaN->empty())
{
	printf("Output of pcl::removeNaNFromPointCloud is dense = %s\n", tmpNoNaN->is_dense?"true":"false");
	*cloud += *util3d::transformPointCloud(tmpNoNaN, iter->second); // transform the point cloud to its pose
	printf("Cloud after transform is dense = %s\n", cloud->is_dense?"true":"false");
}

[ahsteven]

Hmm, I am not sure what I have done. For some reason when I try the
debugging code I get the error namespace pcl has no
member removeNaNFromPointCloud.

I was not using the point cloud anyway so commentingt out the volelize()
part has solved the problem.

Thank you