17 #include <mrpt/3rdparty/do_opencv_includes.h>
41 std::vector<std::shared_ptr<COpenNI2Generic::CDevice>>
vDevices;
46 bool setONI2StreamMode(
47 openni::VideoStream& stream,
int w,
int h,
int fps,
48 openni::PixelFormat
format);
49 std::string oni2DevInfoStr(
const openni::DeviceInfo& info,
int tab = 0);
50 bool cmpONI2Device(
const openni::DeviceInfo& i1,
const openni::DeviceInfo& i2);
51 #endif // MRPT_HAS_OPENNI2
65 : m_rgb_format(openni::PIXEL_FORMAT_RGB888),
66 m_depth_format(openni::PIXEL_FORMAT_DEPTH_1_MM)
69 const char* sVerbose = getenv(
"MRPT_HWDRIVERS_VERBOSE");
70 m_verbose = (sVerbose !=
nullptr) && atoi(sVerbose) != 0;
76 "After initialization:\n %s\n",
77 openni::OpenNI::getExtendedError()));
83 int width,
int height,
float fps,
bool open_streams_now)
88 m_rgb_format(openni::PIXEL_FORMAT_RGB888),
89 m_depth_format(openni::PIXEL_FORMAT_DEPTH_1_MM),
94 m_grab_3D_points(true)
96 const char* sVerbose = getenv(
"MRPT_HWDRIVERS_VERBOSE");
97 m_verbose = (sVerbose !=
nullptr) && atoi(sVerbose) != 0;
105 "After initialization:\n %s\n",
106 openni::OpenNI::getExtendedError()));
123 std::cerr <<
"[" << __FUNCTION__ <<
"]" << std::endl
124 <<
" Initialized OpenNI2." << std::endl;
131 #endif // MRPT_HAS_OPENNI2
148 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
159 std::cerr << message;
169 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
171 openni::Array<openni::DeviceInfo> oni2InfoArray;
172 openni::OpenNI::enumerateDevices(&oni2InfoArray);
174 const size_t numDevices = oni2InfoArray.getSize();
177 " Get device list. %d devices connected.\n", (
int)numDevices));
180 std::set<int> newDevices;
181 for (
unsigned i = 0; i < numDevices; i++)
183 const openni::DeviceInfo& info = oni2InfoArray[i];
185 showLog(oni2DevInfoStr(info, 3) +
"\n");
187 bool isExist =
false;
188 for (
unsigned int j = 0, j_end =
vDevices.size();
189 j < j_end && isExist ==
false; ++j)
191 if (cmpONI2Device(info,
vDevices[j]->getInfo()))
196 if (isExist ==
false)
198 newDevices.insert(i);
202 for (
int newDevice : newDevices)
204 const openni::DeviceInfo& info = oni2InfoArray[newDevice];
211 if (device->getSerialNumber(sn))
214 "Device[%d]: serial number: `%u`\n", newDevice, sn));
221 showLog(
" No devices connected -> EXIT\n");
230 #endif // MRPT_HAS_OPENNI2
235 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
238 openni::OpenNI::shutdown();
241 #endif // MRPT_HAS_OPENNI2
247 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
252 return vDevices[sensor_id]->isOpen();
255 #endif // MRPT_HAS_OPENNI2
261 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
270 "Sensor index is higher than the number of connected devices.");
278 mrpt::format(
" The sensor [%d] is already opened\n", sensor_id));
283 "[COpenNI2Generic] DBG: [%s] about to call vDevices[%d]->open()\n",
284 __FUNCTION__, sensor_id);
290 showLog(
" open successfully.\n");
296 std::this_thread::sleep_for(1000ms);
299 #endif // MRPT_HAS_OPENNI2
303 [maybe_unused]]
const std::set<unsigned>& serial_required)
306 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
308 unsigned num_open_dev = 0;
309 for (
unsigned sensor_id = 0; sensor_id <
vDevices.size(); sensor_id++)
311 unsigned int serialNum;
312 if (
vDevices[sensor_id]->getSerialNumber(serialNum) ==
false)
319 "[COpenNI2Generic::openDevicesBySerialNum] checking device "
320 "with serial '%d'\n",
323 if (serial_required.find(serialNum) == serial_required.end())
337 "[COpenNI2Generic] DBG: [%s] about to call "
338 "vDevices[%d]->open(%d,%d,%d)\n",
339 __FUNCTION__, sensor_id, width, height, (
int)
m_fps);
348 "[COpenNI2Generic] DBG: [%s] now has %d devices open\n",
349 __FUNCTION__, num_open_dev);
354 #endif // MRPT_HAS_OPENNI2
358 const unsigned int SerialRequired)
360 std::set<unsigned> serial_required;
361 serial_required.insert(SerialRequired);
366 [[maybe_unused]]
const unsigned int SerialRequired,
367 [[maybe_unused]]
int& sensor_id)
const
370 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
371 for (
size_t i = 0, i_end =
vDevices.size(); i < i_end; ++i)
374 if (
vDevices[i]->getSerialNumber(sn) ==
false)
378 if (sn == SerialRequired)
387 #endif // MRPT_HAS_OPENNI2
393 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
402 "Sensor index is higher than the number of connected devices.");
407 #endif // MRPT_HAS_OPENNI2
422 [[maybe_unused]]
bool& there_is_obs, [[maybe_unused]]
bool& hardware_error,
423 [[maybe_unused]]
unsigned sensor_id)
434 "Sensor index is higher than the number of connected devices.");
437 rgb_img, timestamp, there_is_obs, hardware_error) ==
false)
445 #endif // MRPT_HAS_OPENNI2
451 [[maybe_unused]]
bool& there_is_obs, [[maybe_unused]]
bool& hardware_error,
452 [[maybe_unused]]
unsigned sensor_id)
463 "Sensor index is higher than the number of connected devices.");
466 depth_img_mm, timestamp, there_is_obs, hardware_error) ==
false)
474 #endif // MRPT_HAS_OPENNI2
487 [[maybe_unused]]
bool& there_is_obs, [[maybe_unused]]
bool& hardware_error,
488 [[maybe_unused]]
unsigned sensor_id)
499 "Sensor index is higher than the number of connected devices.");
502 out_obs, there_is_obs, hardware_error) ==
false)
510 #endif // MRPT_HAS_OPENNI2
515 [[maybe_unused]]
unsigned sensor_id)
const
518 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
519 if (
isOpen(sensor_id) ==
false)
526 #endif // MRPT_HAS_OPENNI2
531 [[maybe_unused]]
unsigned sensor_id)
const
534 std::lock_guard<std::recursive_mutex> lock(
vDevices_mx);
535 if (
isOpen(sensor_id) ==
false)
542 #endif // MRPT_HAS_OPENNI2
555 bool setONI2StreamMode(
556 openni::VideoStream& stream,
int w,
int h,
int fps,
557 openni::PixelFormat
format)
562 const openni::Array<openni::VideoMode>& modes =
563 stream.getSensorInfo().getSupportedVideoModes();
564 for (
int i = 0, i_end = modes.getSize(); i < i_end; ++i)
570 if (modes[i].getResolutionX() != w)
574 if (modes[i].getResolutionY() != h)
578 if (modes[i].getFps() != fps)
582 if (modes[i].getPixelFormat() !=
format)
586 openni::Status rc = stream.setVideoMode(modes[i]);
587 if (rc != openni::STATUS_OK)
596 std::string oni2DevInfoStr(
const openni::DeviceInfo& info,
int tab)
598 std::stringstream sst;
600 for (
int i = 0; i < tab; ++i)
604 sst << space <<
"name=" << info.getName() << std::endl;
605 sst << space <<
"uri=" << info.getUri() << std::endl;
606 sst << space <<
"vendor=" << info.getVendor() << std::endl;
607 sst << space <<
"product=" << info.getUsbProductId();
611 bool cmpONI2Device(
const openni::DeviceInfo& i1,
const openni::DeviceInfo& i2)
613 return (strcmp(i1.getUri(), i2.getUri()) == 0);
616 COpenNI2Generic::CDevice::CDevice(
617 const openni::DeviceInfo& info, openni::PixelFormat rgb,
618 openni::PixelFormat depth,
bool verbose)
619 : m_info(info), m_mirror(true), m_verbose(
verbose)
621 m_streams[COLOR_STREAM] =
622 CStream::create(m_device, openni::SENSOR_COLOR, rgb, m_log,
m_verbose);
623 m_streams[IR_STREAM] =
624 CStream::create(m_device, openni::SENSOR_IR, rgb, m_log,
m_verbose);
625 m_streams[DEPTH_STREAM] = CStream::create(
626 m_device, openni::SENSOR_DEPTH, depth, m_log,
m_verbose);
629 COpenNI2Generic::CDevice::~CDevice() {
close(); }
630 bool COpenNI2Generic::CDevice::synchMirrorMode()
634 for (
auto& m_stream : m_streams)
636 if (!m_stream)
continue;
640 mirror_support = m_stream->isMirrorSupported();
642 catch (std::logic_error& e)
646 if (mirror_support ==
false)
648 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl;
649 m_log <<
" openni::STREAM_PROPERTY_MIRRORING is not supported on "
650 << m_stream->getName() <<
"." << std::endl;
651 m_log <<
" We assume this is MS Kinect and taken images are "
652 "inverted to right and left."
661 for (
auto& m_stream : m_streams)
663 if (!m_stream)
continue;
664 if (m_stream->isMirrorSupported() ==
false)
668 if (m_stream->setMirror(m_mirror) ==
false)
676 bool COpenNI2Generic::CDevice::startStreams()
680 for (
int i = 0; i < STREAM_TYPE_SIZE; ++i)
682 if (!m_streams[i])
continue;
684 printf(
" [%s] calling m_streams[%d]->start()\n", __FUNCTION__, i);
685 if (m_streams[i]->
start() ==
false)
689 " [%s] m_streams[%d]->start() returned FALSE!\n",
698 " [%s] m_streams[%d]->start() returned TRUE\n", __FUNCTION__,
703 " [COpenNI2Generic::CDevice::startStreams()] %d streams were "
710 bool COpenNI2Generic::CDevice::isOpen()
const
712 return (m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid()) ||
713 (m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid());
716 void COpenNI2Generic::CDevice::close()
718 for (
auto& m_stream : m_streams)
720 if (!m_stream)
continue;
726 bool COpenNI2Generic::CDevice::open(
int w,
int h,
int fps)
731 " [COpenNI2Generic::CDevice::open()] Called with w=%i h=%i "
736 openni::Status rc = m_device.open(getInfo().getUri());
737 if (rc != openni::STATUS_OK)
739 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
740 <<
" Failed to open device " << getInfo().getUri() <<
" "
741 << openni::OpenNI::getExtendedError() << std::endl;
744 for (
int i = 0; i < STREAM_TYPE_SIZE; ++i)
746 if (!m_streams[i])
continue;
748 printf(
" [%s] calling m_streams[%d]->open()\n", __FUNCTION__, i);
750 if (m_streams[i]->
open(w, h, fps) ==
false)
754 " [%s] m_streams[%d]->open() returned FALSE\n",
760 " [%s] m_streams[%d]->open() returned OK\n", __FUNCTION__, i);
763 if (synchMirrorMode() ==
false)
769 if (m_streams[DEPTH_STREAM])
772 m_streams[DEPTH_STREAM]->setCloseRange(CloseRange);
773 m_log <<
" Close range: " << (CloseRange ?
"On" :
"Off") << std::endl;
777 printf(
" DBG: checking if imageRegistrationMode is supported\n");
778 if (m_device.isImageRegistrationModeSupported(
779 openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) &&
780 m_streams[DEPTH_STREAM] && m_streams[DEPTH_STREAM]->isValid() &&
781 m_streams[COLOR_STREAM] && m_streams[COLOR_STREAM]->isValid())
786 if (m_device.setImageRegistrationMode(
787 openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) != openni::STATUS_OK)
789 m_log <<
" setImageRegistrationMode() Failed:"
790 << openni::OpenNI::getExtendedError() << endl;
794 m_log <<
" setImageRegistrationMode() Success" << endl;
799 m_log <<
" Device doesn't do image registration!" << endl;
804 m_streams[COLOR_STREAM]->disableAutoExposure();
805 printf(
"DBG: returned from disableAutoExposure()\n");
808 if (startStreams() ==
false)
817 bool COpenNI2Generic::CDevice::getNextFrameRGB(
819 bool& there_is_obs,
bool& hardware_error)
826 openni::VideoFrameRef frame;
827 if (m_streams[COLOR_STREAM]->getFrame(
828 frame, timestamp, there_is_obs, hardware_error) ==
false)
832 copyFrame<openni::RGB888Pixel, mrpt::img::CImage>(frame, img);
838 bool COpenNI2Generic::CDevice::getNextFrameD(
840 bool& there_is_obs,
bool& hardware_error)
847 openni::VideoFrameRef frame;
848 if (m_streams[DEPTH_STREAM]->getFrame(
849 frame, timestamp, there_is_obs, hardware_error) ==
false)
853 copyFrame<openni::DepthPixel, mrpt::math::CMatrix_u16>(frame, depth_mm);
859 bool COpenNI2Generic::CDevice::getNextFrameRGBD(
861 bool& hardware_error)
865 there_is_obs =
false;
866 hardware_error =
false;
878 openni::VideoFrameRef frame[STREAM_TYPE_SIZE];
879 for (
int i = 0; i < STREAM_TYPE_SIZE; ++i)
881 if (!m_streams[i] || !m_streams[i]->isValid())
continue;
882 if (m_streams[i]->getFrame(
883 frame[i], tm, there_is_obs, hardware_error) ==
false)
887 if (there_is_obs ==
false || hardware_error ==
true)
893 const int width = frame[COLOR_STREAM].getWidth();
894 const int height = frame[COLOR_STREAM].getHeight();
895 if ((frame[DEPTH_STREAM].getWidth() != width) ||
896 (frame[DEPTH_STREAM].
getHeight() != height))
898 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
899 <<
" Both frames don't have the same size." << std::endl;
909 resize(obs, width, height);
911 const char*
data[STREAM_TYPE_SIZE] = {
912 (
const char*)frame[COLOR_STREAM].getData(),
913 (
const char*)frame[DEPTH_STREAM].getData()};
914 const int step[STREAM_TYPE_SIZE] = {frame[COLOR_STREAM].getStrideInBytes(),
915 frame[DEPTH_STREAM].getStrideInBytes()};
917 for (
int yc = 0; yc < height; ++yc)
919 const auto* pRgb = (
const openni::RGB888Pixel*)
data[COLOR_STREAM];
920 const auto* pDepth = (
const openni::DepthPixel*)
data[DEPTH_STREAM];
921 for (
int xc = 0; xc < width; ++xc, ++pDepth, ++pRgb)
931 data[COLOR_STREAM] += step[COLOR_STREAM];
932 data[DEPTH_STREAM] += step[DEPTH_STREAM];
940 const openni::DeviceInfo& info, openni::PixelFormat rgb,
941 openni::PixelFormat depth,
bool verbose)
943 return std::make_shared<CDevice>(info, rgb, depth,
verbose);
946 bool COpenNI2Generic::CDevice::getSerialNumber(std::string& sn)
951 if (isOpened ==
false)
953 rc = m_device.open(getInfo().getUri());
954 if (rc != openni::STATUS_OK)
956 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
957 <<
" Failed to open device " << getInfo().getUri() <<
" "
958 << openni::OpenNI::getExtendedError() << std::endl;
962 char serialNumber[16];
963 rc = m_device.getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
964 if (rc != openni::STATUS_OK)
966 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
967 <<
" Failed to getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER) "
968 << getInfo().getUri() <<
" " << openni::OpenNI::getExtendedError()
972 sn = std::string(serialNumber);
973 if (isOpened ==
false)
980 bool COpenNI2Generic::CDevice::getSerialNumber(
unsigned int& sn)
983 if (getSerialNumber(str) ==
false)
987 std::stringstream sst;
995 std::ostream& log,
bool verbose)
998 m_strName(
"Unknown"),
1003 if (m_type == openni::SENSOR_COLOR)
1005 m_strName =
"openni::SENSOR_COLOR";
1007 else if (m_type == openni::SENSOR_DEPTH)
1009 m_strName =
"openni::SENSOR_DEPTH";
1011 else if (m_type == openni::SENSOR_IR)
1013 m_strName =
"openni::SENSOR_IR";
1017 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1018 <<
" Unknown SensorType -> " << m_type << std::endl;
1022 COpenNI2Generic::CDevice::CStream::~CStream() { destroy(); }
1023 bool COpenNI2Generic::CDevice::CStream::isMirrorSupported()
const
1025 if (isValid() ==
false)
1029 return m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING);
1032 bool COpenNI2Generic::CDevice::CStream::setMirror(
bool flag)
1034 if (isValid() ==
false)
1036 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1037 <<
" " << getName() <<
" is not opened." << std::endl;
1040 if (m_stream.isPropertySupported(openni::STREAM_PROPERTY_MIRRORING) ==
1045 if (m_stream.setMirroringEnabled(flag) != openni::STATUS_OK)
1047 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1048 <<
" setMirroringEnabled() failed: "
1049 << openni::OpenNI::getExtendedError() << std::endl;
1055 bool COpenNI2Generic::CDevice::CStream::isValid()
const
1057 return m_stream.isValid();
1060 void COpenNI2Generic::CDevice::CStream::destroy() { m_stream.destroy(); }
1061 void COpenNI2Generic::CDevice::CStream::setCloseRange(
int& value)
1065 " [CDevice::CStream::setCloseRange] entry with value=%d\n",
1067 m_stream.setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, value);
1070 " [CDevice::CStream::setCloseRange] returned from "
1071 "mstream.setProperty()\n");
1072 m_stream.getProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, &value);
1075 " [CDevice::CStream::setCloseRange] returned from "
1076 "mstream.getProperty() ... value %d\n",
1080 bool COpenNI2Generic::CDevice::CStream::open(
int w,
int h,
int fps)
1083 if (m_type != openni::SENSOR_COLOR && m_type != openni::SENSOR_DEPTH &&
1084 m_type != openni::SENSOR_IR)
1086 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1087 <<
" Unknown SensorType -> " << m_type << std::endl;
1092 " [COpenNI2Generic::CDevice::CStream::open] opening sensor "
1093 "stream with m_type == %d\n",
1095 openni::Status rc = openni::STATUS_OK;
1102 rc = m_stream.create(m_device, m_type);
1103 if (rc != openni::STATUS_OK)
1105 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1106 <<
" Couldn't find sensor " << m_strName <<
":"
1107 << openni::OpenNI::getExtendedError() << std::endl;
1108 if (m_type == openni::SENSOR_COLOR)
1110 m_type = openni::SENSOR_IR;
1111 m_strName =
"openni::SENSOR_IR";
1113 printf(
"DBG: changing type to SENSOR_IR (%d)\n", (
int)m_type);
1114 rc = m_stream.create(m_device, m_type);
1119 if (m_verbose) printf(
"returned OK from stream.create()\n");
1120 openni::VideoMode options = m_stream.getVideoMode();
1121 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl;
1122 m_log <<
" " << m_strName << std::endl;
1125 "Initial resolution (%d, %d) FPS %d Format %d",
1126 options.getResolutionX(), options.getResolutionY(),
1127 options.getFps(), options.getPixelFormat())
1129 if (m_verbose) printf(
"DBG: calling setONI2StreamMode()\n");
1130 if (setONI2StreamMode(m_stream, w, h, fps, m_format) ==
false)
1132 m_log <<
" Can't find desired mode in the " << getName() << std::endl;
1136 if (m_verbose) printf(
"DBG: returned OK from setONI2StreamMode()\n");
1137 if (m_verbose) printf(
"DBG: calling stream.getVideoMode()\n");
1138 options = m_stream.getVideoMode();
1141 "-> (%d, %d) FPS %d Format %d", options.getResolutionX(),
1142 options.getResolutionY(), options.getFps(),
1143 options.getPixelFormat())
1147 " [COpenNI2Generic::CDevice::CStream::open] returning TRUE\n");
1151 bool COpenNI2Generic::CDevice::CStream::start()
1153 if (isValid() ==
false)
1155 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1156 <<
" " << getName() <<
" is not opened." << std::endl;
1159 if (m_stream.start() != openni::STATUS_OK)
1161 m_log <<
"[" << __FUNCTION__ <<
"]" << std::endl
1162 <<
" Couldn't start " << getName()
1163 <<
" stream:" << openni::OpenNI::getExtendedError() << std::endl;
1170 COpenNI2Generic::CDevice::CStream::Ptr
1171 COpenNI2Generic::CDevice::CStream::create(
1173 openni::PixelFormat
format, std::ostream& log,
bool verbose)
1175 return std::make_shared<CStream>(device, type,
format, log,
verbose);
1178 bool COpenNI2Generic::CDevice::CStream::getFrame(
1180 bool& there_is_obs,
bool& hardware_error)
1182 there_is_obs =
false;
1183 hardware_error =
false;
1184 if (isValid() ==
false)
1188 openni::Status rc = m_stream.readFrame(&frame);
1189 if (rc != openni::STATUS_OK)
1191 hardware_error =
true;
1192 std::string message =
1193 mrpt::format(
"Failed to grab frame from %s", getName().c_str());
1196 there_is_obs =
true;
1201 #endif // MRPT_HAS_OPENNI2