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

0.16.3: Added OKVIS support (tested only on EuRoC dataset). Added IMU/IMUThread classes. Added OdomOKVIS/ConfigPath and Rtabmap/ImagesAlreadyRectified parameters. MainWindow, limited odom local feature map to maximum 50 meters from current pose (to avoid VTK glitching with near/far clipping plane).

Browse Source
This commit is contained in:
matlabbe
2018-03-07 19:43:30 -05:00
parent 2fad881202
commit 4969ece356
30 changed files with 1700 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
guilib/src/OdometryViewer.cpp
View File

@@ -344,23 +344,28 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
// 3d features
if(featuresShown_->isChecked())
{
if(!odom.info().localMap.empty())
if(!odom.info().localMap.empty() && !odom.pose().isNull())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud->resize(odom.info().localMap.size());
int i=0;
for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
{
(*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y;
(*cloud)[i].z = iter->second.z;
// filter very far features from current location
if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
{
(*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y;
(*cloud)[i].z = iter->second.z;
// green = inlier, yellow = outliers
bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
(*cloud)[i].r = inlier?0:255;
(*cloud)[i].g = 255;
(*cloud)[i++].b = 0;
// green = inlier, yellow = outliers
bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
(*cloud)[i].r = inlier?0:255;
(*cloud)[i].g = 255;
(*cloud)[i++].b = 0;
}
}
cloud->resize(i);
if(!cloudView_->addCloud("featuresOdom", cloud))
{