Added interpolate parameter to util3d::getDepth()

Added Reset calibration button
Added Sense 3D scanner input source

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

corelib/include/rtabmap/core/util3d.h

@@ -94,10 +94,12 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
-pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
+pcl::PointXYZ RTABMAP_EXP getDepth(
-					   int x, int y,
+		const cv::Mat & depthImage,
-					   float cx, float cy,
+		float x, float y,
-					   float fx, float fy);
+		float cx, float cy,
+		float fx, float fy,
+		bool interpolate);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/Features2d.cpp

@@ -68,7 +68,7 @@ void filterKeypointsByDepth(
 		int oi=0;
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy);
+			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy, true);
 			if(uIsFinite(pt.z) && pt.z < maxDepth)
 			{
 				output[oi++] = keypoints[i];

corelib/src/util3d.cpp

@@ -312,12 +312,13 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
 	{
 		pcl::PointXYZ pt = util3d::getDepth(
 				depth,
-				iter->second.pt.x+0.5f,
+				iter->second.pt.x,
-				iter->second.pt.y+0.5f,
+				iter->second.pt.y,
 				cx,
 				cy,
 				fx,
-				fy);
+				fy,
+				true);
 
 		if(!transform.isNull() && !transform.isIdentity())
 		{
@@ -414,14 +415,23 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }
 
-pcl::PointXYZ getDepth(const cv::Mat & depthImage,
+pcl::PointXYZ getDepth(
-					   int x, int y,
+		const cv::Mat & depthImage,
-			       float cx, float cy,
+		float x, float y,
-			       float fx, float fy)
+		float cx, float cy,
+		float fx, float fy,
+		bool interpolate)
 {
-	UASSERT(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows);
-
 	pcl::PointXYZ pt;
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	if(!(int(x) >=0 && int(x)<depthImage.cols && int(y) >=0 && int(y)<depthImage.rows))
+	{
+		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f, y=%f, cols=%d, rows=%d)",
+				x,y,depthImage.cols, depthImage.rows);
+		pt.x = pt.y = pt.z = bad_point;
+		return pt;
+	}
 
 	// Use correct principal point from calibration
 	float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
@@ -433,31 +443,88 @@ pcl::PointXYZ getDepth(const cv::Mat & depthImage,
 	float unit_scaling = isInMM?0.001f:1.0f;
 	float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
 	float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
-	float bad_point = std::numeric_limits<float>::quiet_NaN ();
 
-	float depth;
+	float depth = 0.0f;
-	bool isValid;
+	if(!interpolate || (int(x) < 1 || int(y) < 1 || int(x) >= depthImage.cols-1 || int(y) >= depthImage.rows-1))
-	if(isInMM)
 	{
-		depth = (float)depthImage.at<uint16_t>(y,x);
+		if(interpolate)
-		isValid = depth != 0.0f;
+		{
+			UERROR("Cannot interpolate for points on the image side. Falling back to no interpolation.");
+		}
+
+		// select directly to corresponding pixel
+		depth = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
 	}
 	else
 	{
-		depth = depthImage.at<float>(y,x);
+		// Interpolate x axis
-		isValid = uIsFinite(depth);
+		float depthX;
+		float first;
+		float second;
+		if(int(x) == int(x+0.5f))
+		{
+			first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)-1):depthImage.at<float>(int(y),int(x)-1);
+			second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
+		}
+		else
+		{
+			first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
+			second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)+1):depthImage.at<float>(int(y),int(x)+1);
+		}
+
+		if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
+		{
+			// y = ax + b...
+			float a = second-first;
+			float b = first - a*(float(int(x))-0.5f);
+			depthX = a*(x) + b;
+			//UDEBUG("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthX);
+		}
+
+		if(depthX != 0.0f)
+		{
+			depth = depthX;
+		}
+		else
+		{
+			// Interpolate y axis
+			float depthY;
+			if(int(y) == int(y+0.5f))
+			{
+				first = isInMM?(float)depthImage.at<uint16_t>(int(y)-1,int(x)):depthImage.at<float>(int(y)-1,int(x));
+				second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
+			}
+			else
+			{
+				first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
+				second = isInMM?(float)depthImage.at<uint16_t>(int(y)+1,int(x)):depthImage.at<float>(int(y)+1,int(x));
+			}
+
+			if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
+			{
+				// y = ax + b...
+				float a = second-first;
+				float b = first - a*(float(int(y))-0.5f);
+				depthY = a*(y) + b;
+				//UWARN("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthY);
+			}
+			if(depthY != 0.0f)
+			{
+				depth = depthY;
+			}
+		}
 	}
 
 	// Check for invalid measurements
-	if (!isValid)
+	if (depth==0.0f || !uIsFinite(depth))
 	{
 		pt.x = pt.y = pt.z = bad_point;
 	}
 	else
 	{
 		// Fill in XYZ
-		pt.x = (float(x) - center_x) * depth * constant_x;
+		pt.x = (x - center_x) * depth * constant_x;
-		pt.y = (float(y) - center_y) * depth * constant_y;
+		pt.y = (y - center_y) * depth * constant_y;
 		pt.z = depth*unit_scaling;
 	}
 	return pt;
@@ -642,7 +709,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
-			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy);
+			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
 			pt.x = ptXYZ.x;
 			pt.y = ptXYZ.y;
 			pt.z = ptXYZ.z;
@@ -705,7 +772,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.r = v;
 			}
 
-			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy);
+			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
 			pt.x = ptXYZ.x;
 			pt.y = ptXYZ.y;
 			pt.z = ptXYZ.z;
@@ -944,8 +1011,8 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
 		iter!=correspondences.end();
 		++iter)
 	{
-		pcl::PointXYZ pt1 = getDepth(depthImage1, int(iter->first.x+0.5f), int(iter->first.y+0.5f), cx, cy, fx, fy);
+		pcl::PointXYZ pt1 = getDepth(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
-		pcl::PointXYZ pt2 = getDepth(depthImage2, int(iter->second.x+0.5f), int(iter->second.y+0.5f), cx, cy, fx, fy);
+		pcl::PointXYZ pt2 = getDepth(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
 		if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
 		   (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
 		{
@@ -1470,7 +1537,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
 	for(unsigned int i=0; i<kpts.size(); ++i)
 	{
-		pcl::PointXYZ pt = getDepth(imageDepth, int(kpts[i].pt.x+0.5f), int(kpts[i].pt.y+0.5f), (float)image.cols/2, (float)image.rows/2, 1.0f/constant, 1.0f/constant);
+		pcl::PointXYZ pt = getDepth(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
 		if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
 		{
 			points->push_back(pt);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -228,6 +228,7 @@ private slots:
 	void cleanOdometryTest();
 	void testOdometry();
 	void calibrate();
+	void resetCalibration();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );

guilib/src/MainWindow.cpp

@@ -297,6 +297,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
 	connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
+	connect(_ui->actionOpenNI2_Sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+	_ui->actionOpenNI2_Sense->setEnabled(CameraOpenNI2::available());
 
 	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
 	connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));

guilib/src/PreferencesDialog.cpp

@@ -242,6 +242,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_openniCx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_openniCy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
+	connect(_ui->pushButton_calibrate_reset, SIGNAL(clicked()), this, SLOT(resetCalibration()));
 
 
 	//Rtabmap basic
@@ -1655,7 +1656,7 @@ void PreferencesDialog::selectSourceRGBD(Src src)
 				tr("Activate RGB-D SLAM?"),
 				tr("You've selected RGB-D camera as source input, "
 				   "would you want to activate RGB-D SLAM mode?"),
-				QMessageBox::Yes | QMessageBox::No);
+				QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
 		if(button & QMessageBox::Yes)
 		{
 			_ui->general_checkBox_activateRGBD->setChecked(true);
@@ -1673,6 +1674,27 @@ void PreferencesDialog::selectSourceRGBD(Src src)
 	{
 		_ui->groupBox_sourceImage->setChecked(false);
 		_ui->groupBox_sourceDatabase->setChecked(false);
+
+		if(_ui->doubleSpinBox_openniFx->value() != 0 ||
+			_ui->doubleSpinBox_openniFy->value() != 0 ||
+			_ui->doubleSpinBox_openniCx->value() != 0 ||
+			_ui->doubleSpinBox_openniCy->value() != 0 )
+		{
+			int button = QMessageBox::information(this,
+					tr("Calibration detected"),
+					tr("Some calibration values (fx=%1, fy=%2, cx=%3, cy=%4) are set.\n"
+					   "Do you want to reset them to factory defaults?")
+					   .arg(_ui->doubleSpinBox_openniFx->value())
+					   .arg(_ui->doubleSpinBox_openniFy->value())
+					   .arg(_ui->doubleSpinBox_openniCx->value())
+					   .arg(_ui->doubleSpinBox_openniCy->value()),
+					QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+			if(button & QMessageBox::Yes)
+			{
+				this->resetCalibration();
+			}
+		}
+
 		if(validateForm())
 		{
 			this->writeSettings();
@@ -2988,4 +3010,12 @@ void PreferencesDialog::calibrate()
 	}
 }
 
+void PreferencesDialog::resetCalibration()
+{
+	_ui->doubleSpinBox_openniFx->setValue(0);
+	_ui->doubleSpinBox_openniFy->setValue(0);
+	_ui->doubleSpinBox_openniCx->setValue(0);
+	_ui->doubleSpinBox_openniCy->setValue(0);
+}
+
 }

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>21</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -110,8 +110,15 @@
        <addaction name="actionOpenNI2"/>
        <addaction name="actionOpenNI_CV_ASUS"/>
       </widget>
+      <widget class="QMenu" name="menuSense_3D_scanner">
+       <property name="title">
+        <string>Sense 3D scanner</string>
+       </property>
+       <addaction name="actionOpenNI2_Sense"/>
+      </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
+      <addaction name="menuSense_3D_scanner"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuImage"/>
@@ -1056,6 +1063,11 @@
     <string>View scans...</string>
    </property>
   </action>
+  <action name="actionOpenNI2_Sense">
+   <property name="text">
+    <string>OpenNI2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>732</width>
+            <width>736</width>
-            <height>1198</height>
+            <height>751</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>0</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -287,14 +287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       <property name="title">
                        <string>Save/load settings</string>
                       </property>
-                      <layout class="QVBoxLayout" name="verticalLayout_9">
+                      <layout class="QHBoxLayout" name="horizontalLayout_9">
-                       <item>
-                        <widget class="QPushButton" name="pushButton_saveConfig">
-                         <property name="text">
-                          <string>Save settings (*.ini) ...</string>
-                         </property>
-                        </widget>
-                       </item>
                        <item>
                         <widget class="QPushButton" name="pushButton_loadConfig">
                          <property name="text">
@@ -302,6 +295,13 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item>
+                        <widget class="QPushButton" name="pushButton_saveConfig">
+                         <property name="text">
+                          <string>Save settings (*.ini) ...</string>
+                         </property>
+                        </widget>
+                       </item>
                        <item>
                         <widget class="QPushButton" name="pushButton_resetConfig">
                          <property name="text">
@@ -309,6 +309,19 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item>
+                        <spacer name="horizontalSpacer_4">
+                         <property name="orientation">
+                          <enum>Qt::Horizontal</enum>
+                         </property>
+                         <property name="sizeHint" stdset="0">
+                          <size>
+                           <width>40</width>
+                           <height>20</height>
+                          </size>
+                         </property>
+                        </spacer>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -1821,11 +1834,35 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </layout>
                           </item>
                           <item>
-                           <widget class="QPushButton" name="pushButton_calibrate">
+                           <layout class="QHBoxLayout" name="horizontalLayout_8">
-                            <property name="text">
+                            <item>
-                             <string>Calibrate</string>
+                             <widget class="QPushButton" name="pushButton_calibrate">
-                            </property>
+                              <property name="text">
-                           </widget>
+                               <string>Calibrate</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item>
+                             <widget class="QPushButton" name="pushButton_calibrate_reset">
+                              <property name="text">
+                               <string>Reset</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item>
+                             <spacer name="horizontalSpacer_3">
+                              <property name="orientation">
+                               <enum>Qt::Horizontal</enum>
+                              </property>
+                              <property name="sizeHint" stdset="0">
+                               <size>
+                                <width>40</width>
+                                <height>20</height>
+                               </size>
+                              </property>
+                             </spacer>
+                            </item>
+                           </layout>
                           </item>
                          </layout>
                         </widget>