diff --git a/.cproject b/.cproject
index 538dd697..c1f3a547 100644
--- a/.cproject
+++ b/.cproject
@@ -1,610 +1,619 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- cmake
- -E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../
- true
- false
- true
-
-
- cmake
- -E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=ON ../
- true
- false
- true
-
-
- cmake
- -E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=OFF ../
- true
- false
- true
-
-
- cmake
- -E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=OFF ../
- true
- false
- true
-
-
- cmake
- -E chdir build/ cmake -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../
- true
- false
- true
-
-
- cmake
- -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Release ../
-
- true
- false
- true
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ cmake
+ -E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../
+ true
+ false
+ true
+
+
+ cmake
+ -E chdir build/ cmake -G "MinGW Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=ON ../
+ true
+ false
+ true
+
+
+ cmake
+ -E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=OFF ../
+ true
+ false
+ true
+
+
+ cmake
+ -E chdir build/ cmake -G "Unix Makefiles" -D CMAKE_BUILD_TYPE=Release -D BUILD_TESTS=OFF ../
+ true
+ false
+ true
+
+
+ cmake
+ -E chdir build/ cmake -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Debug -D BUILD_TESTS=ON ../
+ true
+ false
+ true
+
+
+ cmake
+ -G "NMake Makefiles" -D CMAKE_BUILD_TYPE=Release ../
+
+ true
+ false
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0df4cff..28a2f536 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -137,6 +137,7 @@ FIND_PACKAGE(PCL 1.7 REQUIRED)
FIND_PACKAGE(VTK REQUIRED)
FIND_PACKAGE(ZLIB REQUIRED)
FIND_PACKAGE(Freenect)
+FIND_PACKAGE(OpenNI2)
# If Qt is here, the GUI will be built
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
@@ -319,9 +320,17 @@ ENDIF(NOT WIN32)
IF(APPLE)
MESSAGE(STATUS " BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
ENDIF(APPLE)
+
IF(Freenect_FOUND)
-MESSAGE(STATUS " Freenect found.")
+MESSAGE(STATUS " With Freenect = YES")
ELSE()
-MESSAGE(STATUS " Freenect (libfreenect) not found.")
+MESSAGE(STATUS " With Freenect = NO (libfreenect not found)")
ENDIF()
+
+IF(OpenNI2_FOUND)
+MESSAGE(STATUS " With OpenNI2 = YES")
+ELSE()
+MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)")
+ENDIF()
+
MESSAGE(STATUS "--------------------------------------------")
diff --git a/cmake_modules/FindOpenNI2.cmake b/cmake_modules/FindOpenNI2.cmake
new file mode 100644
index 00000000..b0839f88
--- /dev/null
+++ b/cmake_modules/FindOpenNI2.cmake
@@ -0,0 +1,29 @@
+# - Find OpenNI2
+# This module finds an installed OpenNI2 package.
+#
+# It sets the following variables:
+# OpenNI2_FOUND - Set to false, or undefined, if OpenNI2 isn't found.
+# OpenNI2_INCLUDE_DIRS - The OpenNI2 include directory.
+# OpenNI2_LIBRARIES - The OpenNI2 library to link against.
+
+FIND_PATH(OpenNI2_INCLUDE_DIRS OpenNI.h HINTS $ENV{OPENNI2_INCLUDE64} $ENV{OPENNI2_INCLUDE})
+
+FIND_LIBRARY(OpenNI2_LIBRARY NAMES OpenNI2 HINTS $ENV{OPENNI2_LIB64} $ENV{OPENNI2_LIB})
+
+IF (OpenNI2_INCLUDE_DIRS AND OpenNI2_LIBRARY)
+ SET(OpenNI2_FOUND TRUE)
+ENDIF (OpenNI2_INCLUDE_DIRS AND OpenNI2_LIBRARY)
+
+IF (OpenNI2_FOUND)
+ # show which OpenNI2 was found only if not quiet
+ SET(OpenNI2_LIBRARIES ${OpenNI2_LIBRARY})
+ IF (NOT OpenNI2_FIND_QUIETLY)
+ MESSAGE(STATUS "Found OpenNI2")
+ ENDIF (NOT OpenNI2_FIND_QUIETLY)
+ELSE (OpenNI2_FOUND)
+ # fatal error if OpenNI2 is required but not found
+ IF (OpenNI2_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find OpenNI2")
+ ENDIF (OpenNI2_FIND_REQUIRED)
+ENDIF (OpenNI2_FOUND)
+
diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h
index be7addc3..69da9d1f 100644
--- a/corelib/include/rtabmap/core/Camera.h
+++ b/corelib/include/rtabmap/core/Camera.h
@@ -31,6 +31,12 @@
class UDirectory;
class UTimer;
+namespace openni
+{
+class Device;
+class VideoStream;
+}
+
namespace rtabmap
{
@@ -165,6 +171,9 @@ class RTABMAP_EXP CameraRGBD :
public Camera
{
+public:
+ static bool available();
+
public:
CameraRGBD(float imageRate = 0,
bool asus = false);
@@ -178,6 +187,33 @@ protected:
private:
bool _asus;
cv::VideoCapture _capture;
+ float _depthFocal;
+};
+
+/////////////////////////
+// CameraOpenNI2
+/////////////////////////
+class RTABMAP_EXP CameraOpenNI2 :
+ public Camera
+{
+
+public:
+ static bool available();
+
+public:
+ CameraOpenNI2(float imageRate = 0);
+ virtual ~CameraOpenNI2();
+
+ virtual bool init();
+
+protected:
+ virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+
+private:
+ openni::Device * _device;
+ openni::VideoStream * _color;
+ openni::VideoStream * _depth;
+ float _depthFocal;
};
diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt
index 6be7872d..acc8e864 100644
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -64,6 +64,18 @@ IF(Freenect_FOUND)
)
ENDIF(Freenect_FOUND)
+IF(OpenNI2_FOUND)
+ ADD_DEFINITIONS("-DWITH_OPENNI2")
+ SET(INCLUDE_DIRS
+ ${INCLUDE_DIRS}
+ ${OpenNI2_INCLUDE_DIRS}
+ )
+ SET(LIBRARIES
+ ${LIBRARIES}
+ ${OpenNI2_LIBRARIES}
+ )
+ENDIF(OpenNI2_FOUND)
+
####################################
# Generate resources files
####################################
diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp
index 29dd1db0..efaa0eb4 100644
--- a/corelib/src/Camera.cpp
+++ b/corelib/src/Camera.cpp
@@ -31,6 +31,12 @@
#include
#include
+#include
+
+#ifdef WITH_OPENNI2
+
+#endif
+#include
namespace rtabmap
{
@@ -382,9 +388,16 @@ void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCons
/////////////////////////
// CameraRGBD
/////////////////////////
+bool CameraRGBD::available()
+{
+ printf("\nOpencV build: \n", cv::getBuildInformation().c_str());
+ return true;
+}
+
CameraRGBD::CameraRGBD(float imageRate, bool asus) :
Camera(imageRate),
- _asus(asus)
+ _asus(asus),
+ _depthFocal(0.0f)
{
}
@@ -406,12 +419,14 @@ bool CameraRGBD::init()
if(_capture.isOpened())
{
_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
+ _depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
// Print some avalible device settings.
UINFO("Depth generator output mode:");
UINFO("FRAME_WIDTH %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
UINFO("FRAME_HEIGHT %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
UINFO("FRAME_MAX_DEPTH %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
UINFO("FPS %f", _capture.get( CV_CAP_PROP_FPS ));
+ UINFO("Focal %f", _depthFocal);
UINFO("REGISTRATION %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
{
@@ -450,7 +465,202 @@ void CameraRGBD::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConst
depth = depth.clone();
rgb = rgb.clone();
- depthConstant = 0.001905f;
+ UASSERT(_depthFocal > 0.0f);
+ depthConstant = 1.0f/_depthFocal;
+ }
+ else
+ {
+ ULOGGER_WARN("The camera must be initialized before requesting an image.");
+ }
+}
+
+
+/////////////////////////
+// CameraOpenNI2
+/////////////////////////
+bool CameraOpenNI2::available()
+{
+#ifdef WITH_OPENNI2
+ return true;
+#else
+ return false;
+#endif
+}
+
+CameraOpenNI2::CameraOpenNI2(float imageRate) :
+ Camera(imageRate),
+ _device(new openni::Device()),
+ _color(new openni::VideoStream()),
+ _depth(new openni::VideoStream()),
+ _depthFocal(0.0f)
+{
+}
+
+CameraOpenNI2::~CameraOpenNI2()
+{
+ _color->stop();
+ _color->destroy();
+ _depth->stop();
+ _depth->destroy();
+ _device->close();
+ openni::OpenNI::shutdown();
+
+ delete _device;
+ delete _color;
+ delete _depth;
+}
+
+bool CameraOpenNI2::init()
+{
+ openni::OpenNI::initialize();
+
+ if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
+ {
+ UERROR("CameraOpenNI2: Cannot open device.");
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
+ {
+ UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
+ _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
+ {
+ UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
+ {
+ UERROR("CameraOpenNI2: Cannot create depth stream.");
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
+ {
+ UERROR("CameraOpenNI2: Cannot create color stream.");
+ _depth->destroy();
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
+ {
+ UERROR("CameraOpenNI2: Failed to set depth/color registration.");
+ }
+
+ _depth->setMirroringEnabled(false);
+ _color->setMirroringEnabled(false);
+
+ const openni::Array& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
+ for(int i=0; i& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
+ for(int i=0; isetVideoMode(mMode);
+
+ openni::VideoMode mModeColor;
+ mModeColor.setFps(30);
+ mModeColor.setResolution(640,480);
+ mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
+ _color->setVideoMode(mModeColor);
+
+ UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+ _depth->getVideoMode().getFps(),
+ _depth->getVideoMode().getPixelFormat(),
+ _depth->getVideoMode().getResolutionX(),
+ _depth->getVideoMode().getResolutionY(),
+ _depth->getHorizontalFieldOfView(),
+ _depth->getVerticalFieldOfView());
+
+ _depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+ UINFO("depth focal = %f", _depthFocal);
+
+ UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+ _color->getVideoMode().getFps(),
+ _color->getVideoMode().getPixelFormat(),
+ _color->getVideoMode().getResolutionX(),
+ _color->getVideoMode().getResolutionY(),
+ _color->getHorizontalFieldOfView(),
+ _color->getVerticalFieldOfView());
+
+ if(_depth->start() != openni::STATUS_OK ||
+ _color->start() != openni::STATUS_OK)
+ {
+ UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
+ _depth->stop();
+ _color->stop();
+ _depth->destroy();
+ _color->destroy();
+ _device->close();
+ openni::OpenNI::shutdown();
+ return false;
+ }
+
+ uSleep(1000); // just to make sure the sensor is correctly initialized
+
+ return true;
+}
+
+void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+ if(_device->isValid() &&
+ _depth->isValid() &&
+ _color->isValid() &&
+ _device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
+ _device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
+ {
+ openni::VideoFrameRef depthFrame, colorFrame;
+
+ _depth->readFrame(&depthFrame);
+ _color->readFrame(&colorFrame);
+
+ if(depthFrame.isValid() && colorFrame.isValid())
+ {
+ int h=depthFrame.getHeight();
+ int w=depthFrame.getWidth();
+ depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
+
+ h=colorFrame.getHeight();
+ w=colorFrame.getWidth();
+ cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
+ cv::cvtColor(tmp, rgb, CV_RGB2BGR);
+ }
+ UASSERT(_depthFocal != 0.0f);
+ depthConstant = 1.0f/_depthFocal;
}
else
{
diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h
index bcc6eef9..35015b60 100644
--- a/guilib/include/rtabmap/gui/MainWindow.h
+++ b/guilib/include/rtabmap/gui/MainWindow.h
@@ -117,6 +117,7 @@ private slots:
void selectFreenect();
void selectOpenniCv();
void selectOpenniCvAsus();
+ void selectOpenni2();
void dumpTheMemory();
void dumpThePrediction();
void downloadAllClouds();
diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
index 0527da3b..815bc844 100644
--- a/guilib/include/rtabmap/gui/PreferencesDialog.h
+++ b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -80,7 +80,8 @@ public:
kSrcOpenNI_PCL,
kSrcFreenect,
kSrcOpenNI_CV,
- kSrcOpenNI_CV_ASUS
+ kSrcOpenNI_CV_ASUS,
+ kSrcOpenNI2
};
enum OdomType {
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index 832fe335..823618c5 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -286,6 +286,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
selectSourceImageGrp->addAction(_ui->actionFreenect);
selectSourceImageGrp->addAction(_ui->actionOpenNI_CV);
selectSourceImageGrp->addAction(_ui->actionOpenNI_CV_ASUS);
+ selectSourceImageGrp->addAction(_ui->actionOpenNI2);
this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
@@ -298,6 +299,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionFreenect->setEnabled(CameraFreenect::available());
connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
+ _ui->actionOpenNI_CV->setEnabled(CameraRGBD::available());
+ _ui->actionOpenNI_CV_ASUS->setEnabled(CameraRGBD::available());
+ connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+ _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));
@@ -1841,6 +1846,7 @@ void MainWindow::updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src sr
_ui->actionFreenect->setChecked(used && src == PreferencesDialog::kSrcFreenect);
_ui->actionOpenNI_CV->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV);
_ui->actionOpenNI_CV_ASUS->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV_ASUS);
+ _ui->actionOpenNI2->setChecked(used && src == PreferencesDialog::kSrcOpenNI2);
}
void MainWindow::changeImgRateSetting()
@@ -2068,11 +2074,22 @@ void MainWindow::startDetection()
}
}
else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
- _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
+ _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS ||
+ _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
{
- Camera * camera = new CameraRGBD(
- _preferencesDialog->getGeneralInputRate(),
- _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
+ Camera * camera = 0;
+ if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+ {
+ camera = new CameraOpenNI2(
+ _preferencesDialog->getGeneralInputRate());
+ }
+ else
+ {
+ camera = new CameraRGBD(
+ _preferencesDialog->getGeneralInputRate(),
+ _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
+ }
+
camera->setLocalTransform(_preferencesDialog->getSourceOpenniLocalTransform());
if(!camera->init())
@@ -2620,6 +2637,11 @@ void MainWindow::selectOpenniCvAsus()
_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
}
+void MainWindow::selectOpenni2()
+{
+ _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
+}
+
void MainWindow::dumpTheMemory()
{
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index c823c994..620efdbc 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -238,6 +238,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->radioButton_freenect->setEnabled(CameraFreenect::available());
connect(_ui->radioButton_opennicv, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->radioButton_opennicvasus, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+ _ui->radioButton_opennicv->setEnabled(CameraRGBD::available());
+ _ui->radioButton_opennicvasus->setEnabled(CameraRGBD::available());
+ connect(_ui->radioButton_openni2, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+ _ui->radioButton_openni2->setEnabled(CameraOpenNI2::available());
connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1611,6 +1615,7 @@ void PreferencesDialog::selectSourceRGBD(Src src)
_ui->radioButton_freenect->setChecked(src == kSrcFreenect);
_ui->radioButton_opennicv->setChecked(src == kSrcOpenNI_CV);
_ui->radioButton_opennicvasus->setChecked(src == kSrcOpenNI_CV_ASUS);
+ _ui->radioButton_openni2->setChecked(src == kSrcOpenNI2);
if(_obsoletePanels)
{
@@ -2533,6 +2538,10 @@ PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
{
return kSrcOpenNI_CV_ASUS;
}
+ else if (_ui->radioButton_openni2->isChecked())
+ {
+ return kSrcOpenNI2;
+ }
else
{
return kSrcOpenNI_PCL;
@@ -2752,11 +2761,23 @@ void PreferencesDialog::testOdometry(OdomType type)
_odomCameraFreenect = 0;
}
}
- else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
+ else if(this->getSourceRGBD() == kSrcOpenNI_CV ||
+ this->getSourceRGBD() == kSrcOpenNI_CV_ASUS ||
+ this->getSourceRGBD() == kSrcOpenNI2)
{
- Camera * camera = new CameraRGBD(
+ Camera * camera = 0;
+ if(this->getSourceRGBD() == kSrcOpenNI2)
+ {
+ camera = new CameraOpenNI2(
+ this->getGeneralInputRate());
+
+ }
+ else
+ {
+ camera = new CameraRGBD(
this->getGeneralInputRate(),
this->getSourceRGBD() == kSrcOpenNI_CV_ASUS);
+ }
camera->setLocalTransform(this->getSourceOpenniLocalTransform());
_odomCameraOpenNICv = new CameraThread(camera);
if(!_odomCameraOpenNICv->init())
diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui
index 07606e28..78f94a54 100644
--- a/guilib/src/ui/mainWindow.ui
+++ b/guilib/src/ui/mainWindow.ui
@@ -94,6 +94,7 @@
+
@@ -1012,6 +1013,11 @@
OpenNI-CV-ASUS
+
+
+ OpenNI2
+
+
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 67352853..939c5c0f 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -63,9 +63,9 @@
0
- 0
+ -331
609
- 811
+ 657
@@ -1763,6 +1763,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
+ -
+
+
+ OpenNI2
+
+
+
-