Added CameraInfo class (added Camera tab in Statistics panel). Scans/userData are not saved anymore when Mem/BinDataKept=false. StereoACameraImages: Fixed error when scan path is not set. Refactoring of OdometryOpticalFlow class to provide variance when 3D->2D estimation is used.

corelib/src/Camera.cpp

@@ -61,7 +61,7 @@ Camera::~Camera()
 	}
 }
 
-SensorData Camera::takeImage()
+SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;
 	float actualFrameRate = 0;
@@ -91,14 +91,20 @@ SensorData Camera::takeImage()
 
 	UTimer timer;
 	SensorData data  = this->captureImage();
+	double captureTime = timer.ticks();
 	if(warnFrameRateTooHigh)
 	{
 		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-				_imageRate, actualFrameRate, timer.ticks());
+				_imageRate, actualFrameRate, captureTime);
 	}
 	else
 	{
-		UDEBUG("Time capturing image = %fs", timer.ticks());
+		UDEBUG("Time capturing image = %fs", captureTime);
+	}
+	if(info)
+	{
+		info->id_ = data.id();
+		info->timeCapture_ = captureTime;
 	}
 	return data;
 }

corelib/src/CameraRGB.cpp

@@ -66,7 +66,8 @@ CameraImages::CameraImages(const std::string & path,
 	_count(0),
 	_dir(0),
 	_countScan(0),
-	_scanDir(0)
+	_scanDir(0),
+	_scanMaxPts(0)
 {
 
 }
@@ -152,6 +153,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	}
 	if(!_scanPath.empty())
 	{
+		UINFO("scan path=%s", _scanPath.c_str());
 		_scanDir = new UDirectory(_scanPath, "pcd bin"); // "bin" is for KITTI format
 		if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
 		{

corelib/src/CameraThread.cpp

@@ -67,9 +67,9 @@ void CameraThread::setImageRate(float imageRate)
 
 void CameraThread::mainLoop()
 {
-	UTimer timer;
 	UDEBUG("");
-	SensorData data = _camera->takeImage();
+	CameraInfo info;
+	SensorData data = _camera->takeImage(&info);
 
 	if(!data.imageRaw().empty())
 	{
@@ -79,6 +79,7 @@ void CameraThread::mainLoop()
 		}
 		if(_mirroring && data.cameraModels().size() == 1)
 		{
+			UTimer timer;
 			cv::Mat tmpRgb;
 			cv::flip(data.imageRaw(), tmpRgb, 1);
 			data.setImageRaw(tmpRgb);
@@ -98,9 +99,11 @@ void CameraThread::mainLoop()
 				cv::flip(data.depthRaw(), tmpDepth, 1);
 				data.setDepthOrRightRaw(tmpDepth);
 			}
+			info.timeMirroring_ = timer.ticks();
 		}
 		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
 		{
+			UTimer timer;
 			cv::Mat depth = util2d::depthFromDisparity(
 					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
 					data.stereoCameraModel().left().fx(),
@@ -108,9 +111,11 @@ void CameraThread::mainLoop()
 			data.setCameraModel(data.stereoCameraModel().left());
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
+			info.timeDisparity_ = timer.ticks();
+			UINFO("Computing disparity = %f s", info.timeDisparity_);
 		}
-
-		this->post(new CameraEvent(data, _camera->getSerial()));
+		info.cameraName_ = _camera->getSerial();
+		this->post(new CameraEvent(data, info));
 	}
 	else if(!this->isKilled())
 	{

corelib/src/Memory.cpp

@@ -3548,13 +3548,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else
 	{
-		rtabmap::CompressionThread ctDepth2d(laserScan);
-		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		ctDepth2d.start();
-		ctUserData.start();
-		ctDepth2d.join();
-		ctUserData.join();
-
 		s = new Signature(id,
 			_idMapCount,
 			isIntermediateNode?-1:0, // tag intermediate nodes as weight=-1
@@ -3563,25 +3556,23 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			pose,
 			stereoCameraModel.isValid()?
 				SensorData(
-						ctDepth2d.getCompressedData(),
-						maxLaserScanMaxPts,
-						data.laserScanMaxRange(),
+						cv::Mat(),
+						0,
+						0,
 						cv::Mat(),
 						cv::Mat(),
 						stereoCameraModel,
 						id,
-						0,
-						ctUserData.getCompressedData()):
+						0):
 				SensorData(
-						ctDepth2d.getCompressedData(),
-						maxLaserScanMaxPts,
-						data.laserScanMaxRange(),
+						cv::Mat(),
+						0,
+						0,
 						cv::Mat(),
 						cv::Mat(),
 						cameraModels,
 						id,
-						0,
-						ctUserData.getCompressedData()));
+						0));
 	}
 	s->setWords(words);
 	s->setWords3(words3D);

corelib/src/Odometry.cpp

@@ -62,6 +62,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 	Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
+	UASSERT(_minInliers >= 1);
 	Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);

corelib/src/OdometryBOW.cpp

@@ -65,6 +65,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
 	int nn = Parameters::defaultVisNNType();
 	float nndr = Parameters::defaultVisNNDR();
 	int featureType = Parameters::defaultVisFeatureType();
@@ -120,7 +121,6 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 		// init the local map with a all 3D features contained in the database
 		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
 		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
-		customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
 		_memory = new Memory(customParameters);
 		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
 		{
@@ -257,7 +257,7 @@ Transform OdometryBOW::computeTransform(
 								this->getPnPFlags(),
 								this->getPose(),
 								uMultimapToMap(newSignature->getWords3()),
-								&variance,
+								isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers
 								&matches,
 								&inliers);
 					}

corelib/src/OdometryOpticalFlow.cpp

@@ -218,7 +218,9 @@ Transform OdometryOpticalFlow::computeTransform(
 		int ki = 0;
 		for(unsigned int i=0; i<status.size(); ++i)
 		{
-			if(status[i])
+			if(status[i] &&
+			   uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
+			   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
 			{
 				refCorners3DKept->at(ki) = refCorners3D_->at(i);
 				objectPointsKept[ki] = objectPoints[i];
@@ -232,71 +234,123 @@ Transform OdometryOpticalFlow::computeTransform(
 		refCornersKept.resize(ki);
 		newCornersKept.resize(ki);
 
-		if(ki && ki >= this->getMinInliers())
+		correspondences = ki;
+
+		if(correspondences && correspondences >= this->getMinInliers())
 		{
+			// get new 3D points
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3DKept;
+			if(!isVarianceFromInliersCount() || this->getEstimationType() != 1)
+			{
+				// Don't compute the new 3D points if the variance is not required on PnP estimation
+				if(!data.rightRaw().empty())
+				{
+					// stereo
+					newCorners3DKept = util3d::generateKeypoints3DStereo(
+							newCornersKept,
+							newLeftFrame,
+							data.rightRaw(),
+							data.stereoCameraModel().left().fx(),
+							data.stereoCameraModel().baseline(),
+							data.stereoCameraModel().left().cx(),
+							data.stereoCameraModel().left().cy(),
+							data.stereoCameraModel().left().localTransform(),
+							stereoWinSize_,
+							stereoMaxLevel_,
+							stereoIterations_,
+							stereoEps_,
+							stereoMaxSlope_);
+				}
+				else
+				{
+					//depth
+					std::vector<cv::KeyPoint> newCornersKeptKpt;
+					cv::KeyPoint::convert(newCornersKept, newCornersKeptKpt);
+					newCorners3DKept = util3d::generateKeypoints3DDepth(
+							newCornersKeptKpt,
+							data.depthRaw(),
+							data.cameraModels());
+				}
+				UASSERT(newCorners3DKept.get() != 0);
+			}
+
+			std::vector<int> inliersV;
 			if(this->getEstimationType() == 1) // PnP
 			{
-				// find correspondences
 				if(this->isInfoDataFilled() && info)
 				{
 					info->refCorners = refCornersKept;
 					info->newCorners = newCornersKept;
 				}
 
-				correspondences = refCornersKept.size();
-
-				if(correspondences >= this->getMinInliers())
-				{
-					//PnPRansac
-					std::vector<int> inliersV;
-					cv::solvePnPRansac(
-							objectPointsKept,
-							newCornersKept,
-							K,
-							cv::Mat(),
-							rvec,
-							tvec,
-							true,
-							this->getIterations(),
-							this->getPnPReprojError(),
+				//PnPRansac
+				cv::solvePnPRansac(
+						objectPointsKept,
+						newCornersKept,
+						K,
+						cv::Mat(),
+						rvec,
+						tvec,
+						true,
+						this->getIterations(),
+						this->getPnPReprojError(),
 #if CV_MAJOR_VERSION < 3
-							0, // min inliers
+						0, // min inliers
 #else
-							0.99, // confidence
+						0.99, // confidence
 #endif
-							inliersV,
-							this->getPnPFlags());
+						inliersV,
+						this->getPnPFlags());
 
-					cv::Rodrigues(rvec, R);
-					Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
-								   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
-								   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+				cv::Rodrigues(rvec, R);
+				Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+							   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+							   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-					inliers = (int)inliersV.size();
-					if((int)inliersV.size() >= this->getMinInliers())
-					{
-						// make it incremental
-						output = (localTransform * pnp).inverse();
-						variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
-					}
-					else
-					{
-						UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
-					}
+				inliers = (int)inliersV.size();
+				if((int)inliersV.size() >= this->getMinInliers())
+				{
+					// make it incremental
+					output = (localTransform * pnp).inverse();
 
-					if(this->isInfoDataFilled() && info)
+					// compute variance from 3D correspondences error
+					variance = 1;
+					if(!isVarianceFromInliersCount())
 					{
-						info->cornerInliers = inliersV;
+						UASSERT(objectPointsKept.size() == newCorners3DKept->size());
+						std::vector<float> errorSqrdDists(inliersV.size());
+						int oi = 0;
+						for(unsigned int i=0; i<inliersV.size(); ++i)
+						{
+							if(pcl::isFinite(newCorners3DKept->at(inliersV[i])))
+							{
+								const cv::Point3f & objPt = objectPointsKept[inliersV[i]];
+								pcl::PointXYZ newPt = util3d::transformPoint(newCorners3DKept->at(inliersV[i]), output);
+								errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+							}
+						}
+						errorSqrdDists.resize(oi);
+						if(errorSqrdDists.size())
+						{
+							std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+							double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+							variance = 2.1981 * median_error_sqr;
+						}
 					}
 				}
 				else
 				{
-					UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+					UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+				}
+
+				if(this->isInfoDataFilled() && info)
+				{
+					info->cornerInliers = inliersV;
 				}
 			}
 			else
 			{
-				// Get 3D correspondences
+				// Get 3D correspondences (remove NaN)
 				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
 				correspondencesRef->resize(newCornersKept.size());
@@ -307,66 +361,22 @@ Transform OdometryOpticalFlow::computeTransform(
 					info->newCorners.resize(newCornersKept.size());
 				}
 				int oi = 0;
-				if(!data.rightRaw().empty())
+				UASSERT(newCorners3DKept->size() == newCornersKept.size());
+				for(unsigned int i=0; i<newCornersKept.size(); ++i)
 				{
-					// stereo
-					pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
-							newCornersKept,
-							newLeftFrame,
-							data.rightRaw(),
-							data.stereoCameraModel().left().fx(),
-							data.stereoCameraModel().baseline(),
-							data.stereoCameraModel().left().cx(),
-							data.stereoCameraModel().left().cy(),
-							Transform::getIdentity(),
-							stereoWinSize_,
-							stereoMaxLevel_,
-							stereoIterations_,
-							stereoEps_,
-							stereoMaxSlope_);
-
-					UASSERT(newCorners3D->size() == refCorners3DKept->size());
-					for(unsigned int i=0; i<newCorners3D->size(); ++i)
+					if(pcl::isFinite(newCorners3DKept->at(i)) &&
+						(this->getMaxDepth() == 0.0f || newCorners3DKept->at(i).z < this->getMaxDepth()))
 					{
-						if(pcl::isFinite(newCorners3D->at(i)) && (this->getMaxDepth() <= 0.0f || newCorners3D->at(i).z < this->getMaxDepth()))
-						{
-							//Add 3D correspondences!
-							correspondencesRef->at(oi) = refCorners3DKept->at(i);
-							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), localTransform);
-							if(this->isInfoDataFilled() && info)
-							{
-								info->refCorners[oi] = refCornersKept[i];
-								info->newCorners[oi] = newCornersKept[i];
-							}
-							++oi;
-						}
-					}// end loop
-				}
-				else
-				{
-					//depth
-					for(unsigned int i=0; i<newCornersKept.size(); ++i)
-					{
-						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depthRaw().cols)) &&
-						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depthRaw().rows)))
-						{
-							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depthRaw(), newCornersKept[i].x, newCorners[i].y,
-									data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), true);
-							if(pcl::isFinite(pt) &&
-								(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
-							{
-								//Add 3D correspondences!
-								correspondencesRef->at(oi) = refCorners3DKept->at(i);
-								correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform);
+						//Add 3D correspondences!
+						correspondencesRef->at(oi) = refCorners3DKept->at(i);
+						correspondencesNew->at(oi) = newCorners3DKept->at(i);
 
-								if(this->isInfoDataFilled() && info)
-								{
-									info->refCorners[oi] = refCornersKept[i];
-									info->newCorners[oi] = newCornersKept[i];
-								}
-								++oi;
-							}
+						if(this->isInfoDataFilled() && info)
+						{
+							info->refCorners[oi] = refCornersKept[i];
+							info->newCorners[oi] = newCornersKept[i];
 						}
+						++oi;
 					}
 				}
 				correspondencesRef->resize(oi);
@@ -381,7 +391,6 @@ Transform OdometryOpticalFlow::computeTransform(
 
 				if(correspondences >= this->getMinInliers())
 				{
-					std::vector<int> inliersV;
 					UTimer timerRANSAC;
 					Transform t = util3d::transformFromXYZCorrespondences(
 							correspondencesNew,
@@ -402,17 +411,17 @@ Transform OdometryOpticalFlow::computeTransform(
 					{
 						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 					}
-
-					if(this->isInfoDataFilled() && info)
-					{
-						info->cornerInliers = inliersV;
-					}
 				}
 				else
 				{
 					UWARN("Not enough correspondences (%d)", correspondences);
 				}
 			}
+
+			if(this->isInfoDataFilled() && info)
+			{
+				info->cornerInliers = inliersV;
+			}
 		}
 	}
 	else
@@ -458,6 +467,7 @@ Transform OdometryOpticalFlow::computeTransform(
 			newCorners3D->resize(newCorners.size());
 			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
 			int oi=0;
+			UTimer corner3dTimer;
 			if(!data.rightRaw().empty())
 			{
 				/// stereo
@@ -515,6 +525,7 @@ Transform OdometryOpticalFlow::computeTransform(
 					}
 				}
 			}
+			UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
 			newCornersFiltered.resize(oi);
 			newCorners3D->resize(oi);
 

corelib/src/RegistrationVis.cpp

@@ -320,7 +320,7 @@ Transform RegistrationVis::computeTransformation(
 						_PnPFlags,
 						Transform::getIdentity(),
 						uMultimapToMap(*words3To),
-						&variance,
+						_varianceFromInliersCount?0:&variance,
 						0,
 						&inliersV);
 				inliersCount = (int)inliersV.size();

corelib/src/util3d_features.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/EpipolarGeometry.h"
 
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 
 #include <opencv2/video/tracking.hpp>
@@ -71,7 +72,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 		for(unsigned int i=0; i!=keypoints.size(); ++i)
 		{
 			int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
-			UASSERT(cameraIndex < (int)cameraModels.size());
+			UASSERT_MSG(cameraIndex < (int)cameraModels.size(),
+					uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f",
+							cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(
 					depth,
 					keypoints[i].pt.x-subImageWidth*cameraIndex,

corelib/src/util3d_motion_estimation.cpp

@@ -159,6 +159,18 @@ Transform estimateMotion3DTo2D(
 					*varianceOut = 2.1981 * median_error_sqr;
 				}
 			}
+			else if(varianceOut)
+			{
+				// compute variance, which is the rms of reprojection errors
+				std::vector<cv::Point2f> imagePointsReproj;
+				cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
+				float err = 0.0f;
+				for(unsigned int i=0; i<inliers.size(); ++i)
+				{
+					err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
+				}
+				*varianceOut = std::sqrt(err/float(inliers.size()));
+			}
 		}
 	}