CameraOpenni.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
28 #include <rtabmap/utilite/UFile.h>
30 #include <rtabmap/utilite/UTimer.h>
31 #include <opencv2/imgproc/types_c.h>
32 
33 #ifdef RTABMAP_OPENNI
34 #include <pcl/io/openni_grabber.h>
35 #include <pcl/io/oni_grabber.h>
36 #include <pcl/io/openni_camera/openni_depth_image.h>
37 #include <pcl/io/openni_camera/openni_image.h>
38 #endif
39 
40 namespace rtabmap
41 {
42 
43 CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
44  Camera(imageRate, localTransform),
45  interface_(0),
46  deviceId_(deviceId),
47  depthConstant_(0.0f)
48 {
49 }
50 
52 {
53 #ifdef RTABMAP_OPENNI
54  return true;
55 #else
56  return false;
57 #endif
58 }
59 
61 {
62 #ifdef RTABMAP_OPENNI
63  UDEBUG("");
64  if(connection_.connected())
65  {
66  connection_.disconnect();
67  }
68 
69  if(interface_)
70  {
71  interface_->stop();
72  uSleep(1000); // make sure it is stopped
73  delete interface_;
74  interface_ = 0;
75  }
76 #endif
77 }
78 #ifdef RTABMAP_OPENNI
79 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
80 void CameraOpenni::image_cb (
81  const std::shared_ptr<openni_wrapper::Image>& rgb,
82  const std::shared_ptr<openni_wrapper::DepthImage>& depth,
83  float constant)
84 #else
85 void CameraOpenni::image_cb (
88  float constant)
89 #endif
90 {
91  UScopeMutex s(dataMutex_);
92 
93  bool notify = rgb_.empty();
94 
95  cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
96  rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
97  cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
98 
99  depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
100  depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
101 
102  depthConstant_ = constant;
103 
104  if(notify)
105  {
106  dataReady_.release();
107  }
108 }
109 #endif
110 
111 bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
112 {
113 #ifdef RTABMAP_OPENNI
114  if(interface_)
115  {
116  interface_->stop();
117  uSleep(100); // make sure it is stopped
118  delete interface_;
119  interface_ = 0;
120  }
121 
122  try
123  {
124  if(UFile::getExtension(deviceId_).compare("oni") == 0)
125  {
126  interface_ = new pcl::ONIGrabber(deviceId_, false, true);
127  }
128  else
129  {
130  interface_ = new pcl::OpenNIGrabber(deviceId_);
131  }
132 
133 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
134  std::function<void (
135  const std::shared_ptr<openni_wrapper::Image>&,
136  const std::shared_ptr<openni_wrapper::DepthImage>&,
137  float)> f = std::bind (&CameraOpenni::image_cb, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
138 #else
139  boost::function<void (
142  float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
143 #endif
144  connection_ = interface_->registerCallback (f);
145 
146  interface_->start ();
147  }
148  catch(const pcl::IOException& ex)
149  {
150  UERROR("OpenNI exception: %s", ex.what());
151  if(interface_)
152  {
153  delete interface_;
154  interface_ = 0;
155  }
156  return false;
157  }
158  return true;
159 #else
160  UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
161  return false;
162 #endif
163 }
164 
166 {
167 #ifdef RTABMAP_OPENNI
168  return true;
169 #else
170  return false;
171 #endif
172 }
173 
174 std::string CameraOpenni::getSerial() const
175 {
176 #ifdef RTABMAP_OPENNI
177  if(interface_)
178  {
179  return interface_->getName();
180  }
181 #endif
182  return "";
183 }
184 
186 {
188 #ifdef RTABMAP_OPENNI
189  if(interface_ && interface_->isRunning())
190  {
191  if(!dataReady_.acquire(1, 5000))
192  {
193  UWARN("Not received new frames since 5 seconds, end of stream reached!");
194  }
195  else
196  {
198  if(depthConstant_ && !rgb_.empty() && !depth_.empty())
199  {
201  1.0f/depthConstant_, //fx
202  1.0f/depthConstant_, //fy
203  float(rgb_.cols/2) - 0.5f, //cx
204  float(rgb_.rows/2) - 0.5f, //cy
205  this->getLocalTransform(),
206  0,
207  rgb_.size());
209  }
210 
211  depth_ = cv::Mat();
212  rgb_ = cv::Mat();
213  depthConstant_ = 0.0f;
214  }
215  }
216 #else
217  UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
218 #endif
219  return data;
220 }
221 
222 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
compare
bool compare
rtabmap::CameraOpenni::interface_
pcl::Grabber * interface_
Definition: CameraOpenni.h:89
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
UTimer::now
static double now()
Definition: UTimer.cpp:80
boost::shared_ptr
s
RealScalar s
rtabmap::CameraOpenni::dataReady_
USemaphore dataReady_
Definition: CameraOpenni.h:96
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraOpenni::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraOpenni.cpp:111
rtabmap::CameraOpenni::CameraOpenni
CameraOpenni(const std::string &deviceId="", float imageRate=0, const Transform &localTransform=Transform::getIdentity())
Definition: CameraOpenni.cpp:43
UTimer.h
rtabmap::CameraOpenni::getSerial
virtual std::string getSerial() const
Definition: CameraOpenni.cpp:174
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
data
int data[]
UScopeMutex
Definition: UMutex.h:157
CameraOpenni.h
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::CameraOpenni::~CameraOpenni
virtual ~CameraOpenni()
Definition: CameraOpenni.cpp:60
rtabmap::CameraOpenni::connection_
boost::signals2::connection connection_
Definition: CameraOpenni.h:91
rtabmap::Camera
Definition: Camera.h:43
rtabmap::CameraOpenni::deviceId_
std::string deviceId_
Definition: CameraOpenni.h:90
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraOpenni::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraOpenni.cpp:185
UDEBUG
#define UDEBUG(...)
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraOpenni::dataMutex_
UMutex dataMutex_
Definition: CameraOpenni.h:95
rtabmap::CameraOpenni::depthConstant_
float depthConstant_
Definition: CameraOpenni.h:94
rtabmap::CameraOpenni::available
static bool available()
Definition: CameraOpenni.cpp:51
rtabmap::CameraOpenni::isCalibrated
virtual bool isCalibrated() const
Definition: CameraOpenni.cpp:165
rtabmap::CameraOpenni::rgb_
cv::Mat rgb_
Definition: CameraOpenni.h:93
UThreadC.h
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83
rtabmap::CameraOpenni::depth_
cv::Mat depth_
Definition: CameraOpenni.h:92


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07