some fixes for CameraFlyCapture2 driver on Windows, fixed OdometryMono with stereo cameras

corelib/include/rtabmap/core/Odometry.h

@@ -60,7 +60,7 @@ public:
 	int getRefineIterations() const {return _refineIterations;}
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	bool getEstimationType() const {return _estimationType;}
+	int getEstimationType() const {return _estimationType;}
 	double getPnPReprojError() const {return _pnpReprojError;}
 	int  getPnPFlags() const {return _pnpFlags;}
 	const Transform & previousTransform() const {return previousTransform_;}
@@ -179,6 +179,12 @@ private:
 	double flowEps_;
 	int flowMaxLevel_;
 
+	int stereoWinSize_;
+	int stereoIterations_;
+	double stereoEps_;
+	int stereoMaxLevel_;
+	float stereoMaxSlope_;
+
 	Memory * memory_;
 	int localHistoryMaxSize_;
 	float initMinFlow_;
@@ -187,7 +193,7 @@ private:
 	float fundMatrixReprojError_;
 	float fundMatrixConfidence_;
 
-	cv::Mat refDepth_;
+	cv::Mat refDepthOrRight_;
 	std::map<int, cv::Point2f> cornersMap_;
 	std::multimap<int, cv::Point3f> localMap_;
 	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;

corelib/src/CameraRGB.cpp

@@ -258,7 +258,7 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 	}
 	else
 	{
-		uint32_t guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
 		if(guid != 0 && guid != 0xffffffff)
 		{
 			_guid = uFormat("%08x", guid);

corelib/src/CameraStereo.cpp

@@ -605,8 +605,6 @@ SensorData CameraStereoFlyCapture2::captureImage()
 		FlyCapture2::Image grabbedImage;
 		if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
 		{
-			stamp = UTimer::now();
-
 			// right and left image extracted from grabbed image
 			ImageContainer imageCont;
 
@@ -701,10 +699,10 @@ SensorData CameraStereoFlyCapture2::captureImage()
 						triclopsGetBaseline(triclopsCtx_, &baseline);
 
 						StereoCameraModel model(
-								fx
 								fx,
-								cx
-								cy
+								fx,
+								cx,
+								cy,
 								baseline,
 								this->getLocalTransform());
 						data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());

corelib/src/DBDriver.cpp

@@ -571,7 +571,7 @@ void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLink
 		for(std::map<int, Signature*>::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
 		{
 			links.erase(iter->first);
-			for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
+			for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
 				jter!=iter->second->getLinks().end();
 				++jter)
 			{

corelib/src/Memory.cpp

@@ -907,7 +907,7 @@ std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNul
 	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
 	{
 		links.erase(iter->first);
-		for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
+		for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
 			jter!=iter->second->getLinks().end();
 			++jter)
 		{

corelib/src/Odometry.cpp

@@ -162,11 +162,6 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	}
 
 	UASSERT(!data.imageRaw().empty());
-	if(dynamic_cast<OdometryMono*>(this) == 0 && dynamic_cast<OdometryBOW*>(this) == 0)
-	{
-		UERROR("Depth or stereo images required with the odometry selected!");
-		return Transform();
-	}
 
 	if(!data.stereoCameraModel().isValid() &&
 	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))

corelib/src/OdometryMono.cpp

@@ -50,6 +50,11 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	flowIterations_(Parameters::defaultOdomFlowIterations()),
 	flowEps_(Parameters::defaultOdomFlowEps()),
 	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	stereoWinSize_(Parameters::defaultStereoWinSize()),
+	stereoIterations_(Parameters::defaultStereoIterations()),
+	stereoEps_(Parameters::defaultStereoEps()),
+	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
+	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
 	localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
 	initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
 	initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -64,6 +69,12 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
 	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
 
+	Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
+	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
+
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
 	Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
@@ -139,7 +150,7 @@ void OdometryMono::reset(const Transform & initialPose)
 	Odometry::reset(initialPose);
 	memory_->init("", false, ParametersMap());
 	localMap_.clear();
-	refDepth_ = cv::Mat();
+	refDepthOrRight_ = cv::Mat();
 	cornersMap_.clear();
 	keyFrameWords3D_.clear();
 	keyFramePoses_.clear();
@@ -607,7 +618,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 						cv::RANSAC,
 						fundMatrixReprojError_,
 						fundMatrixConfidence_);
-				std::cout << "F=" << F << std::endl;
+				//std::cout << "F=" << F << std::endl;
 
 				if(!F.empty())
 				{
@@ -693,7 +704,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							P0.at<double>(2,2) = 1;
 
 							UDEBUG("Computing P...done!");
-							std::cout << "P=" << P << std::endl;
+							//std::cout << "P=" << P << std::endl;
 
 							cv::Mat R, T;
 							EpipolarGeometry::findRTFromP(P, R, T);
@@ -712,6 +723,41 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 							oi = 0;
 							UASSERT(newCorners.size() == cloud->size());
+
+							pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
+
+							if(refDepthOrRight_.type() == CV_8UC1)
+							{
+								 newCorners3D = util3d::generateKeypoints3DStereo(
+										 refCorners,
+										 refS->sensorData().imageRaw(),
+										 refDepthOrRight_,
+										 cameraModel.fx(),
+										 data.stereoCameraModel().baseline(),
+										 cameraModel.cx(),
+										 cameraModel.cy(),
+										 Transform::getIdentity(),
+										 stereoWinSize_,
+										 stereoMaxLevel_,
+										 stereoIterations_,
+										 stereoEps_,
+										 stereoMaxSlope_ );
+							}
+							else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
+							{
+								std::vector<cv::KeyPoint> tmpKpts;
+								cv::KeyPoint::convert(refCorners, tmpKpts);
+								CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy());
+								 newCorners3D = util3d::generateKeypoints3DDepth(
+										 tmpKpts,
+										 refDepthOrRight_,
+										 m);
+							}
+							else if(!refDepthOrRight_.empty())
+							{
+								UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type());
+							}
+
 							for(unsigned int i=0; i<cloud->size(); ++i)
 							{
 								if(cloud->at(i).z>0)
@@ -719,17 +765,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									imagePoints[oi] = newCorners[i];
 									tmpCornersId[oi] = cornerIds[i];
 									(*inliersRef)[oi] = cloud->at(i);
-									if(!refDepth_.empty())
+									if(!newCorners3D->empty())
 									{
-										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(
-												refDepth_,
-												refCorners[i].x,
-												refCorners[i].y,
-												cameraModel.cx(),
-												cameraModel.cy(),
-												cameraModel.fx(),
-												cameraModel.fy(),
-												true);
+										(*inliersRefGuess)[oi] = newCorners3D->at(i);
 									}
 									++oi;
 								}
@@ -745,7 +783,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							//estimate scale
 							float scale = 1;
 							std::multimap<float, float> scales; // <variance, scale>
-							if(!refDepth_.empty()) // scale known
+							if(!newCorners3D->empty()) // scale known
 							{
 								UASSERT(inliersRefGuess->size() == inliersRef->size());
 								for(unsigned int i=0; i<inliersRef->size(); ++i)
@@ -754,6 +792,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									{
 										float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
 										std::vector<float> errorSqrdDists(inliersRef->size());
+										oi = 0;
 										for(unsigned int j=0; j<inliersRef->size(); ++j)
 										{
 											if(cloud->at(j).z>0)
@@ -763,30 +802,40 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 												refPt.y *= s;
 												refPt.z *= s;
 												const pcl::PointXYZ & guess = inliersRefGuess->at(j);
-												errorSqrdDists[j] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
+												errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
 											}
 										}
-										std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-										double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-										float variance = 2.1981 * median_error_sqr;
-										//UDEBUG("scale %d = %f variance = %f", i, s, variance);
-										if(variance > 0)
+										errorSqrdDists.resize(oi);
+										if(errorSqrdDists.size() > 2)
 										{
-											scales.insert(std::make_pair(variance, s));
+											std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+											double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+											float variance = 2.1981 * median_error_sqr;
+											//UDEBUG("scale %d = %f variance = %f", i, s, variance);
+											if(variance > 0)
+											{
+												scales.insert(std::make_pair(variance, s));
+											}
 										}
 									}
 								}
-								UASSERT(scales.size());
-
-								scale = scales.begin()->second;
-								UDEBUG("scale used = %f (variance=%f)", scale, scales.begin()->first);
-
-								maxVariance_ = 0.01;
-								UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
-								if(scales.begin()->first > 0.01)
+								if(scales.size() == 0)
 								{
-									UWARN("Too high variance %f (should be < 0.01)");
-									reject = true; // 20 cm for good initialization
+									UWARN("No scales found!?");
+									reject = true;
+								}
+								else
+								{
+									scale = scales.begin()->second;
+									UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size());
+
+									maxVariance_ = 0.01;
+									UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
+									if(scales.begin()->first > 0.01)
+									{
+										UWARN("Too high variance %f (should be < 0.01)", scales.begin()->first);
+										reject = true; // 20 cm for good initialization
+									}
 								}
 
 							}
@@ -905,7 +954,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				{
 					cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
 				}
-				refDepth_ = data.depthOrRightRaw().clone();
+				refDepthOrRight_ = data.depthOrRightRaw().clone();
 				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
 			}
 			else

guilib/src/CloudViewer.cpp

@@ -132,7 +132,10 @@ CloudViewer::CloudViewer(QWidget *parent) :
 				-1, 0, 0,
 				0, 0, 0,
 				0, 0, 1);
+#ifndef _WIN32
+	// Crash on startup on Windows (vtk issue)
 	_visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0);
+#endif
 
 	//setup menu/actions
 	createMenu();

guilib/src/PreferencesDialog.cpp

@@ -194,11 +194,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	}
 	if(!CameraStereoDC1394::available())
 	{
-		_ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraStereo->setItemData(0, 0, Qt::UserRole - 1);
 	}
 	if(!CameraStereoFlyCapture2::available())
 	{
-		_ui->comboBox_cameraRGBD->setItemData(7, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraStereo->setItemData(1, 0, Qt::UserRole - 1);
 	}
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
@@ -350,6 +350,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex());
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_rgbd, SLOT(setCurrentIndex(int)));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	_ui->stackedWidget_stereo->setCurrentIndex(_ui->comboBox_cameraStereo->currentIndex());
+	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereo, SLOT(setCurrentIndex(int)));
+	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_autoWhiteBalance, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1040,21 +1043,34 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_useDbStamps->setChecked(false);
 
 #ifdef _WIN32
-		_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2
+		_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2
 #else
 		if(CameraFreenect::available())
 		{
-			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcFreenect-kSrcOpenNI_PCL); // freenect
+			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcFreenect-kSrcRGBD); // freenect
 		}
 		else if(CameraOpenNI2::available())
 		{
-			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcOpenNI_PCL); // openni2
+			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI2-kSrcRGBD); // openni2
 		}
 		else
 		{
-			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI_PCL-kSrcOpenNI_PCL); // openni-pcl
+			_ui->comboBox_cameraRGBD->setCurrentIndex(kSrcOpenNI_PCL-kSrcRGBD); // openni-pcl
 		}
 #endif
+		if(CameraStereoDC1394::available())
+		{
+			_ui->comboBox_cameraStereo->setCurrentIndex(kSrcDC1394-kSrcStereo); // dc1394
+		}
+		else if(CameraStereoFlyCapture2::available())
+		{
+			_ui->comboBox_cameraStereo->setCurrentIndex(kSrcFlyCapture2-kSrcStereo); // flycapture
+		}
+		else
+		{
+			_ui->comboBox_cameraStereo->setCurrentIndex(kSrcStereoImages-kSrcStereo); // stereo images
+		}
+
 		_ui->checkbox_rgbd_colorOnly->setChecked(false);
 		_ui->openni2_autoWhiteBalance->setChecked(true);
 		_ui->openni2_autoExposure->setChecked(true);

guilib/src/ui/mainWindow.ui

@@ -1198,6 +1198,9 @@
    </property>
   </action>
   <action name="actionStereoFlyCapture2">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
    <property name="text">
     <string>FlyCapture2</string>
    </property>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>760</width>
-            <height>1570</height>
+            <width>759</width>
+            <height>887</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -439,7 +439,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="2" column="1">
                           <widget class="QLabel" name="label_161">
                            <property name="text">
-                            <string>Cloud subtraction filtering. When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using &quot;Node filtering&quot; at the same time may generate large &quot;holes&quot; in the map (so better to use without &quot;Node filtering&quot;). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds.</string>
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Cloud subtraction filtering. &lt;span style=&quot; font-weight:600;&quot;&gt;Note that Map's &amp;quot;3D cloud voxel size&amp;quot; parameter below should be set&lt;/span&gt;. &lt;br/&gt;When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using &amp;quot;Node filtering&amp;quot; at the same time may generate large &amp;quot;holes&amp;quot; in the map (so better to use without &amp;quot;Node filtering&amp;quot;). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>