Added parameter "RGBD/ProximityAngle"

Updated default of GuessMotion parameters
Proximity detection by time: added guess transform based on optimized poses
RegistrationVis: ByPass OpticalFlow if no images are detected (use features matching instead if features are set)
solvePnPRansac() refining: using std dev instead of variance
Statistics panel: creating Camera/GUI/Odometry panels without the need to start detection (issue #51)
Statistics: Adding a GT panel when ground truth poses are detected with RMSE info

corelib/include/rtabmap/core/Memory.h

@@ -181,7 +181,7 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 
-	Transform computeTransform(int fromId, int toId, RegistrationInfo * info = 0);
+	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,

corelib/include/rtabmap/core/Parameters.h

@@ -326,6 +326,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
+	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45.0,  "Maximum angle (degrees) for visual proximity detection.");
 
 	// Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -358,7 +359,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, ParticleLambdaR, 		float, 100,       "Lambda of rotational components (roll,pitch,yaw).");
 	RTABMAP_PARAM(Odom, KalmanProcessNoise, 	float, 0.001,     "Process noise covariance value.");
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
-	RTABMAP_PARAM(Odom, GuessMotion,            bool, false,       "Guess next transformation from the last motion computed.");
+	RTABMAP_PARAM(Odom, GuessMotion,            bool, true,       "Guess next transformation from the last motion computed.");
 	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.5,       "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
 
 	// Odometry Bag-of-words
@@ -399,7 +400,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, CorType,                  int, 0,        "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
 	RTABMAP_PARAM(Vis, CorNNType, 	             int, 3,        "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
 	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
-	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 16,        "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
+	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 50,       "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
 	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
 	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
 	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -195,6 +195,7 @@ private:
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
 	bool _proximityScansMerged;
+	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxLinearError;

corelib/src/CameraRGB.cpp

@@ -125,7 +125,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	_count = 0;
 	_countScan = 0;
 	_captureDelay = 0.0;
-	_captureTimer.restart();
 
 	UDEBUG("");
 	if(_dir)
@@ -404,6 +403,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		}
 	}
 
+	_captureTimer.restart();
+
 	return success;
 }
 

corelib/src/Memory.cpp

@@ -2047,6 +2047,7 @@ void Memory::removeRawData(int id)
 Transform Memory::computeTransform(
 		int fromId,
 		int toId,
+		Transform guess,
 		RegistrationInfo * info)
 {
 	const Signature * fromS = this->getSignature(fromId);
@@ -2084,13 +2085,17 @@ Transform Memory::computeTransform(
 				tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 			}
 
-			Transform guess = Transform::getIdentity();
 			if(!_registrationPipeline->isImageRequired())
 			{
 				// no visual in the pipeline, make visual registration for guess
 				RegistrationVis regVis(parameters_);
 				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			}
+			else if(guess.isNull())
+			{
+				guess.setIdentity();
+			}
+
 			if(!guess.isNull())
 			{
 				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);

corelib/src/RegistrationVis.cpp

@@ -288,7 +288,9 @@ Transform RegistrationVis::computeTransformationImpl(
 		std::multimap<int, cv::Point3f> words3To;
 		std::multimap<int, cv::Mat> wordsDescFrom;
 		std::multimap<int, cv::Mat> wordsDescTo;
-		if(_correspondencesApproach == 1) //Optical Flow
+		if(_correspondencesApproach == 1 && //Optical Flow
+		   !fromSignature.sensorData().imageRaw().empty() &&
+		   !toSignature.sensorData().imageRaw().empty())
 		{
 			UDEBUG("");
 			// convert to grayscale
@@ -1054,6 +1056,11 @@ Transform RegistrationVis::computeTransformationImpl(
 			matchesCount = (int)matches[0].size();
 		}
 	}
+	else if(toSignature.sensorData().isValid())
+	{
+		UWARN("Missing correspondences for registration. toWords = %d toImageEmpty=%d",
+				(int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
+	}
 
 	info.inliers = inliersCount;
 	info.matches = matchesCount;

corelib/src/Rtabmap.cpp

@@ -101,6 +101,7 @@ Rtabmap::Rtabmap() :
 	_proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
 	_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
 	_proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
+	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -409,6 +410,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
+	Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
+	_proximityAngle *= M_PI/180.0f;
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
@@ -1169,7 +1172,13 @@ bool Rtabmap::process(
 					std::string rejectedMsg;
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
 					RegistrationInfo info;
-					Transform transform = _memory->computeTransform(signature->id(), *iter, &info);
+					Transform guess;
+					if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
+					{
+						guess = newPose.inverse() * _optimizedPoses.at(*iter);
+					}
+
+					Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info);
 
 					if(!transform.isNull())
 					{
@@ -1722,7 +1731,7 @@ bool Rtabmap::process(
 		info.variance = 1.0f;
 		if(_rgbdSlamMode)
 		{
-			transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
+			transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1814,7 +1823,7 @@ bool Rtabmap::process(
 
 					//find the nearest pose on the path looking in the same direction
 					path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-					path = graph::getPosesInRadius(signature->id(), path, _localRadius, M_PI/4);
+					path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
 					int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 					if(nearestId > 0)
 					{
@@ -1824,7 +1833,7 @@ bool Rtabmap::process(
 							 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
 							RegistrationInfo info;
-							Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
+							Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
 							if(!transform.isNull())
 							{
 								if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)

corelib/src/util3d_motion_estimation.cpp

@@ -92,6 +92,9 @@ Transform estimateMotion3DTo2D(
 	imagePoints.resize(oi);
 	matches.resize(oi);
 
+	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d",
+			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size());
+
 	if((int)matches.size() >= minInliers)
 	{
 		//PnPRansac
@@ -341,9 +344,7 @@ void solvePnPRansac(
 	float inlierThreshold = reprojectionError;
 	if(inliers.size() >= 4 && refineIterations>0)
 	{
-		float inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
 		float error_threshold = inlierThreshold;
-		float sigma_sqr = refineSigma * refineSigma;
 		int refine_iterations = 0;
 		bool inlier_changed = false, oscillating = false;
 		std::vector<int> new_inliers, prev_inliers = inliers;
@@ -393,7 +394,7 @@ void solvePnPRansac(
 			// Estimate the variance and the new threshold
 			float m = uMean(err.data(), err.size());
 			float variance = uVariance(err.data(), err.size());
-			error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
+			error_threshold = std::min(inlierThreshold, refineSigma * float(sqrt(variance)));
 
 			UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
 				  error_threshold, variance, m, refine_iterations, refineIterations);