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

19
corelib/src/OctoMap.cpp
View File

@@ -941,10 +941,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
std::vector<int> * emptyIndices,
std::vector<int> * groundIndices,
bool originalRefPoints,
std::vector<int> * frontierIndices) const
std::vector<int> * frontierIndices,
std::vector<double> * cloudProb) const
{
UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(cloudProb)
{
cloudProb->resize(octree_->size());
}
UDEBUG("depth=%d (maxDepth=%d) octree = %d",
(int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
cloud->resize(octree_->size());
@@ -985,6 +990,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(cloudProb)
{
(*cloudProb)[oi] = it->getOccupancy();
}
if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
{
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
@@ -1031,6 +1040,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(cloudProb)
{
(*cloudProb)[oi] = it->getOccupancy();
}
if(frontierIndices !=0 &&
(!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
@@ -1056,6 +1069,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
}
cloud->resize(oi);
if(cloudProb)
{
cloudProb->resize(oi);
}
if(obstacleIndices)
{
obstacleIndices->resize(si);

4
corelib/src/OdometryThread.cpp
View File

@@ -107,6 +107,10 @@ void OdometryThread::mainLoop()
{
_odometry->reset(_resetPose);
_resetOdometry = false;
UScopeMutex lock(_dataMutex);
_dataBuffer.clear();
_imuBuffer.clear();
_lastImuStamp = 0.0f;
}
SensorData data;

2
corelib/src/RtabmapThread.cpp
View File

@@ -319,7 +319,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
{
if(event->getClassName().compare("IMUEvent") == 0)
{
// IMU events are published at high frequency, exit now
// IMU events are published at high frequency, early exit
return false;
}
else if(event->getClassName().compare("CameraEvent") == 0)

36
corelib/src/StereoCameraModel.cpp
View File

@@ -88,14 +88,18 @@ StereoCameraModel::StereoCameraModel(
{
UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(left_.K_raw(), left_.D_raw(),
right_.K_raw(), right_.D_raw(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
if(left_.imageWidth() == right_.imageHeight())
{
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(
left_.K_raw(), left_.D_raw(),
right_.K_raw(), right_.D_raw(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
}
}
}
@@ -120,14 +124,18 @@ StereoCameraModel::StereoCameraModel(
extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(left_.K_raw(), left_.D_raw(),
right_.K_raw(), right_.D_raw(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
if(left_.imageWidth() == right_.imageHeight())
{
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(
left_.K_raw(), left_.D_raw(),
right_.K_raw(), right_.D_raw(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
}
}
}

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())