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 {
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  {
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());
208  data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
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
virtual bool isCalibrated() const
void release(int n=1)
Definition: USemaphore.h:168
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
f
data
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
std::string getExtension()
Definition: UFile.h:140
CameraOpenni(const std::string &deviceId="", float imageRate=0, const Transform &localTransform=Transform::getIdentity())
virtual std::string getSerial() const
virtual SensorData captureImage(CameraInfo *info=0)
std::string deviceId_
Definition: CameraOpenni.h:94
boost::signals2::connection connection_
Definition: CameraOpenni.h:95
void uSleep(unsigned int ms)
int getNextSeqID()
Definition: Camera.h:86
#define UDEBUG(...)
#define UERROR(...)
#define UWARN(...)
static double now()
Definition: UTimer.cpp:80
model
Definition: trace.py:4
static bool available()
pcl::Grabber * interface_
Definition: CameraOpenni.h:93


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28