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

DatabaseViewer: Saving/loading settings, added 3D view, added Grid from 3d projection parameter

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-04 17:27:44 -05:00
parent c93460aadb
commit 8fd0f3761a
20 changed files with 1101 additions and 411 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/util3d.h
View File

@@ -128,21 +128,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
int decimation);
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
int decimation);
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
float cx, float cy,
float fx, float baseline,
int decimation);
int decimation = 1);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,

28
corelib/src/Graph.cpp
View File

@@ -139,7 +139,7 @@ void optimizeTOROGraph(
std::list<std::map<int, Transform> > * intermediateGraphes)
{
optimizedPoses.clear();
if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
if(depthGraph.size() >= 2 && poses.size()>=2 && links.size()>=1)
{
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
@@ -184,7 +184,7 @@ void optimizeTOROGraph(
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
UASSERT(uContains(rtabmapToToro, iter->first) && uContains(rtabmapToToro, iter->second.to()));
UASSERT(!iter->second.transform().isNull());
UASSERT_MSG(!iter->second.transform().isNull(), uFormat("Link from=%d to=%d", iter->first, iter->second.to()).c_str());
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.rotVariance(), iter->second.transVariance())));
}
}
@@ -225,12 +225,30 @@ void optimizeTOROGraph(
}
else
{
UERROR("No TORO poses and constraints!?");
if(edgeConstraintsToro.size() == 0)
{
UERROR("No TORO constraints!? (input poses=%d, links=%d, depthGraph=%d)",
(int)poses.size(), (int)links.size(), (int)depthGraph.size());
}
if(posesToro.size() == 0)
{
UERROR("No TORO poses!? (input poses=%d, links=%d, depthGraph=%d)",
(int)poses.size(), (int)links.size(), (int)depthGraph.size());
}
}
}
else if(links.size() == 0 && poses.size() == 1)
else if(depthGraph.size() == 1)
{
optimizedPoses = poses;
std::map<int, Transform>::const_iterator iter = poses.find(depthGraph.begin()->first);
if(iter != poses.end())
{
UASSERT_MSG(!iter->second.isNull(), uFormat("Poses should not be null! Id=%d", iter->first).c_str());
optimizedPoses.insert(*iter);
}
else
{
UERROR("Pose %d from depthGraph not found in the poses map!", depthGraph.begin()->first);
}
}
else
{

13
corelib/src/Memory.cpp
View File

@@ -2614,7 +2614,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
}
}
newS->setLabel(oldS->getLabel());
oldS->setLabel("");
oldS->removeLinks(); // remove all links
oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location
@@ -3840,7 +3841,10 @@ void Memory::getMetricConstraints(
{
Transform pose;
this->getPose(ids[i], pose, lookInDatabase);
poses.insert(std::make_pair(ids[i], pose));
if(!pose.isNull())
{
poses.insert(std::make_pair(ids[i], pose));
}
}
for(unsigned int i=0; i<ids.size(); ++i)
@@ -3850,7 +3854,7 @@ void Memory::getMetricConstraints(
std::map<int, Link> neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
if(uContains(poses, jter->first))
if(uContains(poses, jter->first) && jter->second.isValid())
{
bool edgeAlreadyAdded = false;
for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
@@ -3873,7 +3877,8 @@ void Memory::getMetricConstraints(
for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
{
if(jter->first < ids[i] &&
uContains(poses, jter->first))
uContains(poses, jter->first) &&
jter->second.isValid()) // null transform means a child (rehearsed location)
{
links.insert(std::make_pair(ids[i],jter->second));
}

15
corelib/src/util3d.cpp
View File

@@ -835,12 +835,23 @@ cv::Mat disparityFromStereoImages(
const cv::Mat & rightImage)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
cv::Mat leftMono;
if(leftImage.channels() == 3)
{
cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = leftImage;
}
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 0, 15);
cv::Mat disparity;
stereo(leftImage, rightImage, disparity, CV_16SC1);
stereo(leftMono, rightImage, disparity, CV_16SC1);
cv::filterSpeckles(disparity, 0, 1000, 16);
return disparity;
}