CameraOpenNI2: Added OpenNI2 stamps/Ids option

corelib/include/rtabmap/core/CameraRGBD.h

@@ -160,6 +160,7 @@ public:
 	bool setExposure(int value);
 	bool setGain(int value);
 	bool setMirroring(bool enabled);
+	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
 
 protected:
 	virtual SensorData captureImage();
@@ -171,6 +172,7 @@ private:
 	float _depthFx;
 	float _depthFy;
 	std::string _deviceId;
+	bool _openNI2StampsAndIDsUsed;
 };
 
 

corelib/src/CameraRGBD.cpp

@@ -390,7 +390,8 @@ CameraOpenNI2::CameraOpenNI2(
 #endif
 	_depthFx(0.0f),
 	_depthFy(0.0f),
-	_deviceId(deviceId)
+	_deviceId(deviceId),
+	_openNI2StampsAndIDsUsed(false)
 {
 }
 
@@ -715,7 +716,14 @@ SensorData CameraOpenNI2::captureImage()
 						float(rgb.cols/2) - 0.5f,  //cx
 						float(rgb.rows/2) - 0.5f,  //cy
 						this->getLocalTransform());
-				data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+				if(_openNI2StampsAndIDsUsed)
+				{
+					data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
+				}
+				else
+				{
+					data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+				}
 			}
 		}
 	}