i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

OctoMap::createCloud() added cloud probability output option.

Browse Source 
CameraRealSense2: added new warnings to debug easier timestamps problems.
GUI: added gravity visualization.
DbViewer: empty clouds in OctoMap mode have color depending on probability of occupancy.
Brought general changes from local xvision branch.
This commit is contained in:
matlabbe
2020-01-22 10:34:21 -05:00
parent f9e818c900
commit ccc5a5be5a
14 changed files with 367 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -184,7 +184,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
UScopeMutex sm(poseMutex_);
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
if(poseBuffer_.size() > 100)
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
@@ -274,9 +274,13 @@ void CameraRealSense2::getPoseAndIMU(
pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
poseConfidence = iterA->second.second;
}
else if(stamp < iterA->first)
{
UWARN("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
}
else
{
UWARN("Could not find poses to interpolate at time %f", stamp);
UWARN("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", stamp, iterA->first, iterB->first);
}
}
poseMutex_.unlock();
@@ -330,7 +334,14 @@ void CameraRealSense2::getPoseAndIMU(
}
else
{
UWARN("Could not find acc data to interpolate at time %f", stamp);
if(stamp < iterA->first)
{
UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
}
else
{
UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
}
imuMutex_.unlock();
return;
}
@@ -386,7 +397,14 @@ void CameraRealSense2::getPoseAndIMU(
}
else
{
UWARN("Could not find gyro data to interpolate at time %f", stamp);
if(stamp < iterA->first)
{
UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
}
else
{
UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
}
imuMutex_.unlock();
return;
}
@@ -1148,6 +1166,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
IMU imu;
unsigned int confidence = 0;
double imuStamp = stamp*1000.0;
UASSERT(info!=0);
getPoseAndIMU(imuStamp, info->odomPose, confidence, imu);
if(odometryProvided_ && !info->odomPose.isNull())