Added Kinect for Azure menu option (#551)

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

corelib/include/rtabmap/core/camera/CameraK4A.h

@@ -34,6 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/UTimer.h"
 
+#ifdef RTABMAP_K4A
+ #include <k4a/k4atypes.h>
+#endif
+
 namespace rtabmap
 {
 
@@ -67,6 +71,11 @@ private:
 private:
 
 #ifdef RTABMAP_K4A
+	uint32_t device_count_;
+	k4a_device_t device_;
+	k4a_device_configuration_t config_;
+	std::string serial_number_;
+
 	void* playbackHandle_;
 	void* transformationHandle_;
 	CameraModel model_;

corelib/src/camera/CameraK4A.cpp

@@ -61,7 +61,15 @@ CameraK4A::CameraK4A(
 	previousStamp_(0.0)
 #endif
 {
-	UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
+#ifdef RTABMAP_K4A
+	device_ = NULL;
+
+	config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+	config_.camera_fps = K4A_FRAMES_PER_SECOND_15;
+	config_.depth_mode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
+	config_.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
+	config_.color_resolution = K4A_COLOR_RESOLUTION_720P;
+#endif
 }
 
 CameraK4A::CameraK4A(
@@ -96,11 +104,13 @@ void CameraK4A::close()
 	{
 		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
 	}
-	/*
+
 	// Shut down the camera when finished with application logic
-	k4a_device_stop_cameras(device);
+        if(device_ != NULL)
-	k4a_device_close(device);
+	{
-	*/
+		k4a_device_stop_cameras(device_);
+		k4a_device_close(device_);
+	}
 #endif
 }
 
@@ -170,19 +180,17 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 	}
 	else if (deviceId_ >= 0)
 	{
-		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
+		device_count_ = k4a_device_get_installed_count();
-		return false;
+		if (device_count_ == 0)
-
-		/*uint32_t count = k4a_device_get_installed_count();
-		if (count == 0)
 		{
 			UERROR("No k4a devices attached!");
 			return false;
 		}
 
+	        UINFO("CameraK4A found k4a device attached");
+
 		// Open the first plugged in Kinect device
-		k4a_device_t device = NULL;
+		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device_)))
-		if (K4A_FAILED(k4a_device_open(K4A_DEVICE_DEFAULT, &device)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -190,27 +198,26 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 
 		// Get the size of the serial number
 		size_t serial_size = 0;
-		k4a_device_get_serialnum(device, NULL, &serial_size);
+		k4a_device_get_serialnum(device_, NULL, &serial_size);
 
 		// Allocate memory for the serial, then acquire it
 		char *serial = (char*)(malloc(serial_size));
-		k4a_device_get_serialnum(device, serial, &serial_size);
+		k4a_device_get_serialnum(device_, serial, &serial_size);
-		UINFO("Opened device: %s", serial);
+		serial_number_.assign(serial, serial_size);
 		free(serial);
 
-		// Configure a stream of 4096x3072 BRGA color data at 15 frames per second
+		UINFO("Opened device: %s", serial_number_.c_str());
-		k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
-		config.camera_fps = K4A_FRAMES_PER_SECOND_15;
-		config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
-		config.color_resolution = K4A_COLOR_RESOLUTION_3072P;
 
 		// Start the camera with the given configuration
-		if (K4A_FAILED(k4a_device_start_cameras(device, &config)))
+		if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
 		{
 			UERROR("Failed to start cameras!");
-			k4a_device_close(device);
+			k4a_device_close(device_);
 			return false;
-		}*/
+		}
+
+		UINFO("Camera started successfully");
+		return false;
 	}
 	
 	return true;
@@ -228,7 +235,10 @@ bool CameraK4A::isCalibrated() const
 std::string CameraK4A::getSerial() const
 {
 #ifdef RTABMAP_K4A
-	return fileName_.empty()?"":fileName_;
+	if(device_ != NULL)
+		return(serial_number_);
+	else
+		return fileName_.empty()?"":fileName_;
 #else
 	return "";
 #endif
@@ -443,7 +453,7 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 	}
 	else
 	{
-		UERROR("CameraK4A: Live camera stream is not yet supported, only recorded mkv files are.");
+		// Camera stream things here
 	}
 
 #else

guilib/include/rtabmap/gui/MainWindow.h

@@ -172,6 +172,7 @@ protected Q_SLOTS:
 	void selectOpenni2();
 	void selectFreenect2();
 	void selectK4W2();
+	void selectK4A();
 	void selectRealSense();
 	void selectRealSense2();
 	void selectRealSense2Stereo();

guilib/src/MainWindow.cpp

@@ -431,6 +431,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
 	connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
+	connect(_ui->actionKinect_for_Azure, SIGNAL(triggered()), this, SLOT(selectK4A()));
 	connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
@@ -451,6 +452,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
 	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
 	_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
+	_ui->actionKinect_for_Azure->setEnabled(false);
 	_ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
@@ -6332,6 +6334,11 @@ void MainWindow::selectK4W2()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4W2);
 }
 
+void MainWindow::selectK4A()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcK4A);
+}
+
 void MainWindow::selectRealSense()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense);

guilib/src/ui/mainWindow.ui

@@ -168,6 +168,16 @@
        <addaction name="actionFreenect2"/>
        <addaction name="actionKinect_for_Windows_SDK_v2"/>
       </widget>
+      <widget class="QMenu" name="menuKinect_K4A">
+       <property name="title">
+        <string>Kinect for Azure</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/kinect_xbox_one.png</normaloff>:/images_kinect_xbox_one.png</iconset>
+       </property>
+       <addaction name="actionKinect_for_Azure"/>
+      </widget>
       <widget class="QMenu" name="menuRealSense_R200">
        <property name="title">
         <string>RealSense R200</string>
@@ -222,6 +232,7 @@
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuSense_3D_scanner"/>
       <addaction name="menuKinect_v2"/>
+      <addaction name="menuKinect_K4A"/>
       <addaction name="menuRealSense_R200"/>
       <addaction name="menuRealSense_ZR300"/>
       <addaction name="menuRealSense_SR300"/>
@@ -1542,6 +1553,14 @@
     <string>Kinect for Windows SDK v2</string>
    </property>
   </action>
+  <action name="actionKinect_for_Azure">
+   <property name="checkable">
+    <bool>false</bool>
+   </property>
+   <property name="text">
+    <string>Kinect for Azure</string>
+   </property>
+  </action>
   <action name="actionRealSense2_D435">
    <property name="checkable">
     <bool>true</bool>