ARCore: features from arcore are used directly for keypoints

app/android/jni/CameraMobile.cpp

@@ -207,6 +207,7 @@ void CameraMobile::mainLoop()
 		// Rotate image depending on the camera orientation
 		if(colorCameraToDisplayRotation_ == ROTATION_90)
 		{
+			UDEBUG("ROTATION_90");
 			cv::Mat rgb, depth;
 			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::flip(data.imageRaw(),rgb,1);
@@ -226,9 +227,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_180)
 		{
+			UDEBUG("ROTATION_180");
 			cv::Mat rgb, depth;
 			cv::flip(data.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
@@ -244,9 +254,18 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_270)
 		{
+			UDEBUG("ROTATION_270");
 			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
 			cv::transpose(data.imageRaw(),rgb);
 			cv::flip(rgb,rgb,1);
@@ -263,6 +282,14 @@ void CameraMobile::mainLoop()
 					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
 			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
 		}
 
 		rtabmap::Transform pose = info.odomPose;

app/android/jni/CameraMobile.h

@@ -94,7 +94,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	virtual void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
 	void setData(const SensorData & data, const Transform & pose);

corelib/src/Memory.cpp

@@ -4260,29 +4260,48 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 			}
 
-			int oldMaxFeatures = _feature2D->getMaxFeatures();
+			bool useProvided3dPoints = false;
-			UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+			if(_useOdometryFeatures && !data.keypoints().empty())
-			ParametersMap tmpMaxFeatureParameter;
-			if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
 			{
-				// The total extracted features should match the number of features used for transformation estimation
+				UDEBUG("Using provided keypoints (%d)", (int)data.keypoints().size());
-				UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+				keypoints = data.keypoints();
-				tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+
-				_feature2D->parseParameters(tmpMaxFeatureParameter);
+				// In case we provided corresponding 3D features
+				if(keypoints.size() == data.keypoints3D().size())
+				{
+					for(size_t i=0; i<keypoints.size(); ++i)
+					{
+						keypoints[i].class_id = i;
+					}
+					useProvided3dPoints = true;
+				}
 			}
-
+			else
-			keypoints = _feature2D->generateKeypoints(
-					imageMono,
-					depthMask);
-
-			if(tmpMaxFeatureParameter.size())
 			{
-				tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+				int oldMaxFeatures = _feature2D->getMaxFeatures();
-				_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+				UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+				ParametersMap tmpMaxFeatureParameter;
+				if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
+				{
+					// The total extracted features should match the number of features used for transformation estimation
+					UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+					tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+					_feature2D->parseParameters(tmpMaxFeatureParameter);
+				}
+
+				keypoints = _feature2D->generateKeypoints(
+						imageMono,
+						depthMask);
+
+				if(tmpMaxFeatureParameter.size())
+				{
+					tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+					_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+				}
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 			}
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
-			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 			t = timer.ticks();
@@ -4414,7 +4433,22 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					UDEBUG("time rectification = %fs", t);
 				}
 
-				if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
+				{
+					UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
+					keypoints3D.resize(keypoints.size());
+					for(size_t i=0; i<keypoints.size(); ++i)
+					{
+						UASSERT(keypoints[i].class_id < data.keypoints3D().size());
+						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
+					}
+				}
+				else if(keypoints.size() == data.keypoints3D().size())
+				{
+					UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
+					keypoints3D = data.keypoints3D();
+				}
+				else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
 				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 				{
 					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);