Removed parameter "LccIcp/HighTransitionalVariance" (Identity covariance is set directly on pose correction and local loop closure detection in space)

Covariance of virtual links added on the path is set to Identity. For those added to keep the path linked to current map, their covariance is set to 100.
DatabaseViewer: set fixed colors on constraints view
This commit is contained in:
Mathieu Labbe
2015-02-25 17:06:52 -05:00
parent eb96fe1249
commit 1d39db2bcc
11 changed files with 174 additions and 109 deletions

View File

@@ -70,12 +70,14 @@ public:
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose = Transform::getIdentity());
const Transform & pose = Transform::getIdentity(),
const QColor & color = Qt::gray);
bool addOrUpdateCloud(
const std::string & id,

View File

@@ -108,10 +108,13 @@ private:
bool updateConstraintView = true);
void updateStereo(const Signature * data);
void updateWordsMatching();
void updateConstraintView(const rtabmap::Link & link,
void updateConstraintView(
const rtabmap::Link & link,
bool updateImageSliders = true,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
bool updateImageSliders = true);
const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
void updateConstraintButtons();
Link findActiveLink(int from, int to);
bool containsLink(

View File

@@ -179,14 +179,15 @@ bool CloudViewer::updateCloudPose(
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
int index = _visualizer->getColorHandlerIndex(id);
this->removeCloud(id);
if(this->addCloud(id, cloud, pose))
if(this->addCloud(id, cloud, pose, color))
{
_visualizer->updateColorHandlerIndex(id, index);
return true;
@@ -198,14 +199,15 @@ bool CloudViewer::updateCloud(
bool CloudViewer::updateCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const Transform & pose)
const Transform & pose,
const QColor & color)
{
if(_addedClouds.contains(id))
{
UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
int index = _visualizer->getColorHandlerIndex(id);
this->removeCloud(id);
if(this->addCloud(id, cloud, pose))
if(this->addCloud(id, cloud, pose, color))
{
_visualizer->updateColorHandlerIndex(id, index);
return true;
@@ -220,7 +222,7 @@ bool CloudViewer::addOrUpdateCloud(
const Transform & pose,
const QColor & color)
{
if(!updateCloud(id, cloud, pose))
if(!updateCloud(id, cloud, pose, color))
{
return addCloud(id, cloud, pose, color);
}
@@ -233,7 +235,7 @@ bool CloudViewer::addOrUpdateCloud(
const Transform & pose,
const QColor & color)
{
if(!updateCloud(id, cloud, pose))
if(!updateCloud(id, cloud, pose, color))
{
return addCloud(id, cloud, pose, color);
}

View File

@@ -700,6 +700,7 @@ void DatabaseViewer::view3DMap()
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
@@ -1109,10 +1110,7 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_loops->blockSignals(true);
ui_->horizontalSlider_loops->setValue(i);
ui_->horizontalSlider_loops->blockSignals(false);
this->updateConstraintView(loopLinks_.at(i),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
false);
this->updateConstraintView(loopLinks_.at(i), false);
}
ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(0);
@@ -1131,10 +1129,7 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(i);
ui_->horizontalSlider_neighbors->blockSignals(false);
this->updateConstraintView(neighborLinks_.at(i),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
false);
this->updateConstraintView(neighborLinks_.at(i), false);
}
ui_->horizontalSlider_loops->blockSignals(true);
ui_->horizontalSlider_loops->setValue(0);
@@ -1424,16 +1419,16 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
// only called when ui_->checkBox_showOptimized state changed
void DatabaseViewer::updateConstraintView()
{
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
false);
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
}
void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
void DatabaseViewer::updateConstraintView(
const rtabmap::Link & linkIn,
bool updateImageSliders,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
bool updateImageSliders)
const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
{
std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
rtabmap::Link link = linkIn;
@@ -1448,7 +1443,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
ui_->checkBox_showOptimized->setEnabled(false);
UASSERT(!t.isNull() && memory_);
ui_->label_constraint->setText(QString("%1 (%2r=%3 %4t=%5)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.rotVariance())).arg(QChar(0xc3, 0x03)).arg(sqrt(link.transVariance())));
ui_->label_type->setNum(link.type());
ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance())));
ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()));
if(link.type() == Link::kNeighbor &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
@@ -1462,12 +1459,12 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
{
ui_->checkBox_showOptimized->setEnabled(true);
Transform topt = iterFrom->second.inverse()*iterTo->second;
Transform delta = t.inverse()*topt;
float diff = topt.getDistance(t);
Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
a = (a *180.0f) / CV_PI;
ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((delta.getNorm()/t.getNorm())*100.0f).arg(a));
ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((diff/t.getNorm())*100.0f).arg(a));
if(ui_->checkBox_showOptimized->isChecked())
{
@@ -1520,19 +1517,19 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
if(ui_->constraintsViewer->isVisible())
{
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
{
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
//cloud 3d
if(!ui_->checkBox_show3DWords->isChecked())
{
@@ -1584,11 +1581,11 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
}
}
else
@@ -1628,7 +1625,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
}
else
{
@@ -1636,7 +1633,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
}
else
{
@@ -1648,31 +1645,44 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
}
}
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(scanA->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
}
}
else
{
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
}
}
if(scanFrom->size() == 0 && scanTo->size() == 0)
{
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(scanA->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
}
}
else
{
if(scanFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
}
if(scanTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
}
}
@@ -1916,6 +1926,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
if(ui_->checkBox_icp_2d->isChecked())
{
//2D
@@ -1925,20 +1937,20 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldLaserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newLaserScan, t);
scanA = util3d::cvMat2Cloud(oldLaserScan);
scanB = util3d::cvMat2Cloud(newLaserScan, t);
//voxelize
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, ui_->doubleSpinBox_icp_voxel->value());
scanA = util3d::voxelize<pcl::PointXYZ>(scanA, ui_->doubleSpinBox_icp_voxel->value());
scanB = util3d::voxelize<pcl::PointXYZ>(scanB, ui_->doubleSpinBox_icp_voxel->value());
}
if(newCloud->size() && oldCloud->size())
if(scanB->size() && scanA->size())
{
transform = util3d::icp2D(newCloud,
oldCloud,
transform = util3d::icp2D(scanB,
scanA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
&hasConverged,
@@ -2087,7 +2099,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
if(ui_->dockWidget_constraints->isVisible())
{
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
this->updateConstraintView(newLink, cloudA, cloudB);
scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, transform);
this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
}
}
}

View File

@@ -495,7 +495,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
_ui->loopClosure_highICPTransitionalVariance->setObjectName(Parameters::kLccIcpHighTransitionalVariance().c_str());
//Odometry

View File

@@ -40,7 +40,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-16</y>
<width>344</width>
<height>81</height>
</rect>
@@ -56,7 +56,7 @@
<item row="0" column="1">
<widget class="QLabel" name="label_parentsA">
<property name="text">
<string>Parents</string>
<string/>
</property>
</widget>
</item>
@@ -70,7 +70,21 @@
<item row="1" column="1">
<widget class="QLabel" name="label_childrenA">
<property name="text">
<string>Children</string>
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_4">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_labelA">
<property name="text">
<string/>
</property>
</widget>
</item>
@@ -150,7 +164,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-16</y>
<width>344</width>
<height>81</height>
</rect>
@@ -166,7 +180,7 @@
<item row="0" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string>Parents</string>
<string/>
</property>
</widget>
</item>
@@ -180,7 +194,21 @@
<item row="1" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string>Children</string>
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_5">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_labelB">
<property name="text">
<string/>
</property>
</widget>
</item>
@@ -354,48 +382,76 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QLabel" name="label_16">
<property name="text">
<string>Transform</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_constraint">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_showOptimized">
<property name="text">
<string>Optimized</string>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_constraint_opt">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="6" column="0">
<widget class="QLabel" name="label_32">
<property name="text">
<string>Show 3D words</string>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="6" column="1">
<widget class="QCheckBox" name="checkBox_show3DWords">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_18">
<property name="text">
<string>σ (rot, trans)</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_variance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_33">
<property name="text">
<string>Type</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_type">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>

View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-203</y>
<width>744</width>
<height>1101</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>21</number>
<number>20</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5662,13 +5662,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="1" column="0">
<widget class="QCheckBox" name="loopClosure_highICPTransitionalVariance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
<property name="suffix">
@@ -5698,16 +5691,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_80">
<property name="text">
<string>Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder. Only used with ICP 2D.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>