Below ground(flat floor) detection

[emspark85]

Hi, I've got a question about the points detected below ground(floor).
I am using realsense R200 as a RGBD camera, and using wheel encoder for odometry.
As you can see from below pictures(octomap), there are points under the floor(blue~purple voxels). I am not sure whether it is normal or parameter settings are wrong.
Could anyone give me a tip to get fix this problem?




All the parameters I have in .ini file are listed below which is referred by rtabmap.launch.

[CalibrationDialog]
board_height = 6
board_square_size = 0.033000000000000002
board_width = 8
geometry = @ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\0\0\0\x5!\0\0\x3N\0\0\0\0\0\0\0\0\0\0\x5!\0\0\x3N\0\0\0\0\0\0\0\0\a\x80)
max_scale = 1


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


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


[Core]
Version = 0.11.13
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 = 10
FREAK\NOctaves = 4
FREAK\OrientationNormalized = true
FREAK\PatternScale = 22
FREAK\ScaleNormalized = true
GFTT\BlockSize = 3
GFTT\K = 0.04
GFTT\MinDistance = 3
GFTT\QualityLevel = 0.001
GFTT\UseHarrisDetector = false
Grid\3D = true
Grid\3DGroundIsObstacle = false
Grid\CellSize = 0.05
Grid\ClusterRadius = 0.1
Grid\DepthDecimation = 4
Grid\DepthMax = 10.0
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\MapFrameProjection = false
Grid\MaxGroundAngle = 45
Grid\MaxGroundHeight = 0.5
Grid\MaxObstacleHeight = 0.0
Grid\MinClusterSize = 10
Grid\MinGroundHeight = 0.1
Grid\NoiseFilteringMinNeighbors = 2
Grid\NoiseFilteringRadius = 0.05
Grid\NormalK = 10
Grid\NormalsSegmentation = true
Grid\Scan2dMaxFilledRange = 4
Grid\Scan2dUnknownSpaceFilled = false
Grid\ScanDecimation = 1
Icp\CorrespondenceRatio = 0.5
Icp\DownsamplingStep = 10
Icp\Epsilon = 0.01
Icp\Iterations = 10
Icp\MaxCorrespondenceDistance = 0.3
Icp\MaxRotation = 0.78
Icp\MaxTranslation = 0
Icp\PointToPlane = true
Icp\PointToPlaneNormalNeighbors = 20
Icp\VoxelSize = 0.3
Kp\BadSignRatio = 0.5
Kp\DetectorStrategy = 6
Kp\DictionaryPath =
Kp\IncrementalDictionary = true
Kp\IncrementalFlann = true
Kp\MaxDepth = 4
Kp\MaxFeatures = 400
Kp\MinDepth = 0.4
Kp\NNStrategy = 1
Kp\NewWordsComparedTogether = true
Kp\NndrRatio = 0.8
Kp\Parallelized = true
Kp\RoiRatios = 0.0 0.0 0.0 0.3
Kp\SubPixEps = 0.02
Kp\SubPixIterations = 0
Kp\SubPixWinSize = 3
Kp\TfIdfLikelihoodUsed = true
Mem\BadSignaturesIgnored = false
Mem\BinDataKept = true
Mem\GenerateIds = true
Mem\ImageKept = false
Mem\ImagePostDecimation = 1
Mem\ImagePreDecimation = 1
Mem\IncrementalMemory = true
Mem\InitWMWithAllNodes = 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.45
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 = 8
ORB\PatchSize = 31
ORB\ScaleFactor = 1.2
ORB\ScoreType = 0
ORB\WTA_K = 2
Odom\AlignWithGround = false
Odom\FillInfoData = true
Odom\FilteringStrategy = 0
Odom\GuessMotion = false
Odom\Holonomic = true
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 = 0
Odom\ScanKeyFrameThr = 0.7
Odom\Strategy = 0
Odom\VisKeyFrameThr = 100
OdomF2M\BundleAdjustment = 0
OdomF2M\BundleAdjustmentMaxFrames = 0
OdomF2M\MaxNewFeatures = 0
OdomF2M\MaxSize = 2000
OdomF2M\ScanMaxSize = 2000
OdomF2M\ScanSubtractRadius = 0.05
OdomMono\InitMinFlow = 100
OdomMono\InitMinTranslation = 0.1
OdomMono\MaxVariance = 0.01
OdomMono\MinTranslation = 0.02
Optimizer\Epsilon = 1e-05
Optimizer\Iterations = 100
Optimizer\Robust = false
Optimizer\Strategy = 0
Optimizer\VarianceIgnored = false
RGBD\AngularUpdate = 0.01
RGBD\CreateOccupancyGrid = false
RGBD\Enabled = true
RGBD\GoalReachedRadius = 0.5
RGBD\GoalsSavedInUserData = false
RGBD\LinearUpdate = 0.01
RGBD\LocalImmunizationRatio = 0.25
RGBD\LocalRadius = 10
RGBD\LoopClosureReextractFeatures = false
RGBD\MaxLocalRetrieved = 2
RGBD\NeighborLinkRefining = false
RGBD\NewMapOdomChangeDistance = 0
RGBD\OptimizeFromGraphEnd = false
RGBD\OptimizeMaxError = 1
RGBD\PlanAngularVelocity = 0
RGBD\PlanLinearVelocity = 0
RGBD\PlanStuckIterations = 0
RGBD\ProximityAngle = 45
RGBD\ProximityBySpace = true
RGBD\ProximityByTime = false
RGBD\ProximityMaxGraphDepth = 0
RGBD\ProximityMaxPaths = 3
RGBD\ProximityPathFilteringRadius = 1
RGBD\ProximityPathMaxNeighbors = 10
RGBD\ProximityPathRawPosesUsed = true
RGBD\ScanMatchingIdsSavedInLinks = true
Reg\Force3DoF = true
Reg\Strategy = 0
Reg\VarianceFromInliersCount = false
Rtabmap\CreateIntermediateNodes = false
Rtabmap\DetectionRate = 1
Rtabmap\ImageBufferSize = 1
Rtabmap\LoopRatio = 0
Rtabmap\LoopThr = 0.11
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 = 700
Rtabmap\VhStrategy = 0
Rtabmap\WorkingDirectory = /home/nvidia/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 = 60
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 = 15
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.6
Vis\CorNNType = 1
Vis\CorType = 0
Vis\EpipolarGeometryVar = 0.02
Vis\EstimationType = 1
Vis\FeatureType = 6
Vis\ForwardEstOnly = true
Vis\InlierDistance = 0.05
Vis\Iterations = 100
Vis\MaxDepth = 4
Vis\MaxFeatures = 1000
Vis\MinDepth = 0
Vis\MinInliers = 8
Vis\PnPFlags = 0
Vis\PnPRefineIterations = 1
Vis\PnPReprojError = 2
Vis\RefineIterations = 5
Vis\RoiRatios = 0.0 0.0 0.0 0.0
Vis\SubPixEps = 0.02
Vis\SubPixIterations = 0
Vis\SubPixWinSize = 3
g2o\Baseline = 0.075
g2o\Optimizer = 0
g2o\PixelVariance = 1
g2o\RobustKernelDelta = 8
g2o\Solver = 0

[matlabbe]

Hi,

Seems like a TF problem (/base_link->/camera_link). Make sure rtabmap is launched with frame_id set to base frame of the robot, not camera_link. If the height of the camera is okay in TF, show in RVIZ the point cloud sent by the camera, and see if the points that should be on the ground matches with the ground plane of RVIZ. You may have to set Reference frame (at to of Displays view in RVIZ) to the base frame of the robot (e.g., base_link or base_footprint). What is your tf tree ($rosrun tf view_frames)?  

cheers,
Mathieu

[emspark85]

Hi, thanks for the reply.

Here's my tf tree.


Below, i'm attaching first few lines of launch file.
I've set frame_id as base_link. And camera height seems fine which is set by static tf.

<launch>
  <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0.065 -0.065 0.0935 0 0 0 1 base_link camera_link 100" />
   
       
  <arg name="stereo"          default="false"/>
 
 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
 
 
  <arg name="localization"            default="false"/>
 
 
  <arg name="cfg"                     default="/opt/ros/kinetic/share/rtabmap_ros/launch/config/rtabmap.ini" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
  <arg name="frame_id"                default="base_link"/>     
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="100"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="rtabmap_args"            default=""/>             
  <arg name="launch_prefix"           default=""/>             
 
 
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
 
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
 
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
 
  <arg name="compressed"              default="true"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
 
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/velodyne_points"/>
   
  <arg name="visual_odometry"         default="false"/>         
  <arg name="odom_topic"              default="/odom"/>         


it still seems not showing correct ground cloud points.


Any other possible mistakes that I'm making from launch file?
 

[matlabbe]

There are white points under the grid. The problem is coming from the camera, which poorly sees the ground. Is the ground reflective?

On rtabmap side, we could force at least that all these points are ground with "Grid/NormalsSegmentation" to false and "Grid/MaxGroundHeight" to 0.01.

[emspark85]

Thanks for your assistance Mat!

My room is kinda reflective, not as much as mirror.
It is a bit shiny where I can see the shapes of boxes roughly that are on the floor.

As you have suggested, I have set Grid/NormalsSegmentation = false and Grid/MaxGroundHeight=0.01.
It seems OK when I put it in rviz as PointCloud2 (seems there is no ground though).

In addition to above question, what is difference between below two topics..

topic 1: /rtabmap/octomap_occupied_space [sensor_msgs/PointCloud2] published by rtabmap
topic 2: /occupied_cells_vis_array [visualization_msgs/MarkerArray] published by octomap_server

They seem to produce different looking octomap in rviz. octomap_server subscribes depth point cloud published by camera.
Are these different because  rtabmap is filtering out unnecessary points?

picture 1: /rtabmap/octomap_occupied_space [sensor_msgs/PointCloud2] published by rtabmap


picture 2: /occupied_cells_vis_array [visualization_msgs/MarkerArray] published by octomap_server

[matlabbe]

Hi,

/rtabmap/octomap_occupied_space contains OctoMap cells that are occupied and obstacles
/occupied_cells_vis_array contains OctoMap cells that are occupied

To make is more clear, since this new commit, the cloud of /rtabmap/octomap_occupied_space will contain all occupied cells (like /occupied_cells_vis_array). I added /rtabmap/octomap_ground and /rtabmap/octomap_obstacles to show occupied cells that are libelled as ground and obstacles respectively.

You can also show /rtabmap/octomap_grid (2d map) to see your points under the ground projected as ground cells in the map.

cheers,
Mathieu

[ipin]

bro, can help on me too? slightly different case my case

matlabbe wrote

Hi,

/rtabmap/octomap_occupied_space contains OctoMap cells that are occupied and obstacles
/occupied_cells_vis_array contains OctoMap cells that are occupied

To make is more clear, since this new commit, the cloud of /rtabmap/octomap_occupied_space will contain all occupied cells (like /occupied_cells_vis_array). I added /rtabmap/octomap_ground and /rtabmap/octomap_obstacles to show occupied cells that are libelled as ground and obstacles respectively.

You can also show /rtabmap/octomap_grid (2d map) to see your points under the ground projected as ground cells in the map.

cheers,
Mathieu