databaseViewer: updated default ICP parameters, and optimized view of the constraints while refining

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1853 f169173b-cf89-36c8-b27e-44dbe73f0c83

guilib/src/DatabaseViewer.cpp

@@ -1055,7 +1055,8 @@ void DatabaseViewer::update(int value,
 						QLabel * labelParents,
 						QLabel * labelChildren,
 						rtabmap::ImageView * view,
-						QLabel * labelId)
+						QLabel * labelId,
+						bool updateConstraintView)
 {
 	UTimer timer;
 	labelIndex->setText(QString::number(value));
@@ -1160,6 +1161,8 @@ void DatabaseViewer::update(int value,
 	updateConstraintButtons();
 	updateWordsMatching();
 
+	if(updateConstraintView)
+	{
 		// update constraint view
 		int from = ids_.at(ui_->horizontalSlider_A->value());
 		int to = ids_.at(ui_->horizontalSlider_B->value());
@@ -1210,6 +1213,7 @@ void DatabaseViewer::update(int value,
 			ui_->horizontalSlider_loops->blockSignals(false);
 			ui_->horizontalSlider_neighbors->blockSignals(false);
 		}
+	}
 
 	if(rect.isValid())
 	{
@@ -1313,7 +1317,9 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
 	this->updateConstraintView(loopLinks_.at(value));
 }
 
-void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
+void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo)
 {
 	std::multimap<int, Link>::iterator iter = findLink(linksRefined_, link.from(), link.to());
 	rtabmap::Transform t = link.transform();
@@ -1327,34 +1333,61 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 
 	ui_->label_constraint->setText(t.prettyPrint().c_str());
 
+	bool updateA = false;
+	bool updateB = false;
+	ui_->horizontalSlider_A->blockSignals(true);
+	ui_->horizontalSlider_B->blockSignals(true);
 	if(ui_->horizontalSlider_A->value() == idToIndex_.value(link.from()))
 	{
 		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
+		updateB=true;
 	}
 	else if(ui_->horizontalSlider_A->value() == idToIndex_.value(link.to()))
 	{
 		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.from()));
+		updateB=true;
 	}
 	else if(ui_->horizontalSlider_B->value() == idToIndex_.value(link.from()))
 	{
 		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.to()));
+		updateA = true;
 	}
 	else if(ui_->horizontalSlider_B->value() == idToIndex_.value(link.to()))
 	{
 		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
+		updateA=true;
 	}
 	else
 	{
-		ui_->horizontalSlider_A->blockSignals(true);
-		ui_->horizontalSlider_B->blockSignals(true);
 		ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
 		ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
+		updateA=updateB=true;
+	}
 	ui_->horizontalSlider_A->blockSignals(false);
 	ui_->horizontalSlider_B->blockSignals(false);
-		sliderAValueChanged(idToIndex_.value(link.from()));
+	if(updateA)
-		sliderBValueChanged(idToIndex_.value(link.to()));
+	{
+		this->update(idToIndex_.value(link.from()),
+					ui_->label_indexA,
+					ui_->label_parentsA,
+					ui_->label_childrenA,
+					ui_->graphicsView_A,
+					ui_->label_idA,
+					false); // don't update constraints view!
+	}
+	if(updateB)
+	{
+		this->update(idToIndex_.value(link.to()),
+					ui_->label_indexB,
+					ui_->label_parentsB,
+					ui_->label_childrenB,
+					ui_->graphicsView_B,
+					ui_->label_idB,
+					false); // don't update constraints view!
 	}
 
+	if(cloudFrom->size() == 0 && cloudTo->size() == 0)
+	{
 		float fxA, fyA, cxA, cyA;
 		float fxB, fyB, cxB, cyB;
 		rtabmap::Transform localTransformA, localTransformB;
@@ -1461,6 +1494,18 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 		{
 			ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
 		}
+	}
+	else
+	{
+		if(cloudFrom->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+		}
+		if(cloudTo->size())
+		{
+			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+		}
+	}
 	ui_->constraintsViewer->render();
 
 	// update buttons
@@ -1703,6 +1748,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
 	std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
 	memory_->getImageDepth(currentLink.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
 
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
@@ -1739,14 +1786,14 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
 		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
 
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
+		cloudA = util3d::getICPReadyCloud(depthA,
 						fxA, fyA, cxA, cyA,
 						ui_->spinBox_icp_decimation->value(),
 						ui_->doubleSpinBox_icp_maxDepth->value(),
 						ui_->doubleSpinBox_icp_voxel->value(),
 						0, // no sampling
 						localTransformA);
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
+		cloudB = util3d::getICPReadyCloud(depthB,
 						fxB, fyB, cxB, cyB,
 						ui_->spinBox_icp_decimation->value(),
 						ui_->doubleSpinBox_icp_maxDepth->value(),
@@ -1811,7 +1858,11 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		{
 			linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
 		}
-		this->updateConstraintView(newLink);
+		if(ui_->dockWidget_constraints->isVisible())
+		{
+			cloudB = util3d::transformPointCloud(cloudB, transform);
+			this->updateConstraintView(newLink, cloudA, cloudB);
+		}
 	}
 	else
 	{

guilib/src/ui/DatabaseViewer.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1076</width>
-    <height>665</height>
+    <height>675</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -565,10 +565,10 @@
           <number>1</number>
          </property>
          <property name="maximum">
-          <number>8</number>
+          <number>16</number>
          </property>
          <property name="value">
-          <number>4</number>
+          <number>8</number>
          </property>
         </widget>
        </item>
@@ -614,7 +614,7 @@
           <double>1.000000000000000</double>
          </property>
          <property name="value">
-          <double>0.005000000000000</double>
+          <double>0.010000000000000</double>
          </property>
         </widget>
        </item>
@@ -670,7 +670,7 @@
           <string/>
          </property>
          <property name="checked">
-          <bool>true</bool>
+          <bool>false</bool>
          </property>
         </widget>
        </item>