CameraAREngine.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 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "CameraAREngine.h"
29 #include "util.h"
33 #include "rtabmap/core/util2d.h"
34 
35 #include <media/NdkImage.h>
36 
37 namespace rtabmap {
38 
39 
41 // CameraAREngine
43 CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing):
44  CameraMobile(smoothing),
45  env_(env),
46  context_(context),
47  activity_(activity),
48  arInstallRequested_(false)
49 {
50  glGenTextures(1, &textureId_);
51 }
52 
54  // Disconnect ARCore service
55  close();
56 
57  glDeleteTextures(1, &textureId_);
58 }
59 
60 std::string CameraAREngine::getSerial() const
61 {
62  return "AREngine";
63 }
64 
65 bool CameraAREngine::init(const std::string & calibrationFolder, const std::string & cameraName)
66 {
67  close();
68 
70 
71  HwArInstallStatus install_status;
72  // If install was not yet requested, that means that we are resuming the
73  // activity first time because of explicit user interaction (such as
74  // launching the application)
75  bool user_requested_install = !arInstallRequested_;
76 
77  // === ATTENTION! ATTENTION! ATTENTION! ===
78  // This method can and will fail in user-facing situations. Your
79  // application must handle these cases at least somewhat gracefully. See
80  // HelloAR Java sample code for reasonable behavior.
81  HwArEnginesApk_requestInstall(env_, activity_, user_requested_install, &install_status);
82 
83  switch (install_status)
84  {
85  case HWAR_INSTALL_STATUS_INSTALLED:
86  break;
87  case HWAR_INSTALL_STATUS_INSTALL_REQUESTED:
88  arInstallRequested_ = true;
89  return false;
90  }
91 
92  // === ATTENTION! ATTENTION! ATTENTION! ===
93  // This method can and will fail in user-facing situations. Your
94  // application must handle these cases at least somewhat gracefully. See
95  // HelloAR Java sample code for reasonable behavior.
96  UASSERT(HwArSession_create(env_, context_, &arSession_) == HWAR_SUCCESS);
98 
99  HwArConfig_create(arSession_, &arConfig_);
101 
102  HwArConfig_setFocusMode(arSession_, arConfig_, HWAR_FOCUS_MODE_FIXED);
103  UASSERT(HwArSession_configure(arSession_, arConfig_) == HWAR_SUCCESS);
104 
105  HwArFrame_create(arSession_, &arFrame_);
106  UASSERT(arFrame_);
107 
108  HwArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_); // May fail?!
109  //UASSERT(arCameraIntrinsics_);
110 
111  HwArPose_create(arSession_, nullptr, &arPose_);
112  UASSERT(arPose_);
113 
116  HwArConfig_setUpdateMode(arSession_, arConfig_, HWAR_UPDATE_MODE_BLOCKING);
117 
119 
120  if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
121  {
122  UERROR("Cannot resume camera!");
123  // In a rare case (such as another camera app launching) the camera may be
124  // given to a different app and so may not be available to this app. Handle
125  // this properly and recreate the session at the next iteration.
126  close();
127  return false;
128  }
129 
130  return true;
131 }
132 
134 {
136  if (arCameraIntrinsics_ != nullptr)
137  {
138  HwArCameraIntrinsics_destroy(arSession_, arCameraIntrinsics_);
139  }
140  arCameraIntrinsics_ = nullptr;
141 
142  if(arSession_!= nullptr)
143  {
144  HwArSession_destroy(arSession_);
145  }
146  arSession_ = nullptr;
147 
148  if(arConfig_!= nullptr)
149  {
150  HwArConfig_destroy(arConfig_);
151  }
152  arConfig_ = nullptr;
153 
154  if (arFrame_ != nullptr)
155  {
156  HwArFrame_destroy(arFrame_);
157  }
158  arFrame_ = nullptr;
159 
160  if (arPose_ != nullptr)
161  {
162  HwArPose_destroy(arPose_);
163  }
164  arPose_ = nullptr;
165 
167 }
168 
169 void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
170 {
171  CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
172  if(arSession_)
173  {
174  int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
175  if (ret > 3) {
176  ret -= 4;
177  }
178 
179  HwArSession_setDisplayGeometry(arSession_, ret, width, height);
180  }
181 }
182 
184 {
186  //LOGI("Capturing image...");
187 
188  pose.setNull();
190  if(!arSession_)
191  {
192  return data;
193  }
194 
195  if(textureId_ == 0)
196  {
197  glGenTextures(1, &textureId_);
198  glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
199  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
200  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
201  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
202  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
203  }
204  if(textureId_!=0)
205  HwArSession_setCameraTextureName(arSession_, textureId_);
206 
207  // Update session to get current frame and render camera background.
208  if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
209  LOGE("CameraAREngine::captureImage() ArSession_update error");
210  return data;
211  }
212 
213  // If display rotation changed (also includes view size change), we need to
214  // re-query the uv coordinates for the on-screen portion of the camera image.
215  int32_t geometry_changed = 0;
216  HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
217  if (geometry_changed != 0 || !uvs_initialized_) {
218  HwArFrame_transformDisplayUvCoords(
222  UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)",
228  uvs_initialized_ = true;
229  }
230 
231  HwArCamera* ar_camera;
232  HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
233 
234  HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
235  HwArCamera_getProjectionMatrix(arSession_, ar_camera,
236  /*near=*/0.1f, /*far=*/100.f,
238 
239  // adjust origin
240  if(!getOriginOffset().isNull())
241  {
243  }
244 
245  HwArTrackingState camera_tracking_state;
246  HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
247 
248  if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
249  {
250  // Get calibration parameters
251  // FIXME: Hard-coded as getting intrinsics with the api fails
252  float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
253  int32_t camWidth=640, camHeight=480;
254  //HwArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
255  //HwArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
256  //HwArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
257  //HwArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &camWidth, &camHeight);
258  LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, camWidth, camHeight);
259 
260  if(fx > 0 && fy > 0 && camWidth > 0 && camHeight > 0 && cx > 0 && cy > 0)
261  {
262  //ArPointCloud * point_cloud;
263  //ArFrame_acquirePointCloud(ar_session_, ar_frame_, &point_cloud);
264 
265  HwArImage * image = nullptr;
266  HwArImage * depthImage = nullptr;
267  HwArStatus statusRgb = HwArFrame_acquireCameraImage(arSession_, arFrame_, &image);
268  HwArStatus statusDepth = HwArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
269  if(statusRgb == HWAR_SUCCESS && statusDepth == HWAR_SUCCESS)
270  {
271  int64_t timestamp_ns;
272  HwArFrame_getTimestamp(arSession_, arFrame_, &timestamp_ns);
273 
274  int planeCount;
275  uint8_t *imageData = nullptr;
276  int len = 0;
277  int stride;
278  int width;
279  int height;
280  const AImage* ndkImageRGB;
281  HwArImage_getNdkImage(image, &ndkImageRGB);
282 
283  AImage_getNumberOfPlanes(ndkImageRGB, &planeCount);
284  AImage_getWidth(ndkImageRGB, &width);
285  AImage_getHeight(ndkImageRGB, &height);
286  AImage_getPlaneRowStride(ndkImageRGB, 0, &stride);
287  AImage_getPlaneData(ndkImageRGB, 0, &imageData, &len);
288  LOGI("RGB: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
289 
290  cv::Mat outputRGB;
291  if(imageData != nullptr && len>0)
292  {
293  cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, cv::COLOR_YUV2BGR_NV21);
294  }
295 
296  //Depth
297  const AImage* ndkImageDepth;
298  HwArImage_getNdkImage(depthImage, &ndkImageDepth);
299  AImage_getNumberOfPlanes(ndkImageDepth, &planeCount);
300  AImage_getWidth(ndkImageDepth, &width);
301  AImage_getHeight(ndkImageDepth, &height);
302  AImage_getPlaneRowStride(ndkImageDepth, 0, &stride);
303  AImage_getPlaneData(ndkImageDepth, 0, &imageData, &len);
304  LOGI("Depth: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
305 
306  cv::Mat outputDepth(height, width, CV_16UC1);
307  uint16_t *dataShort = (uint16_t *)imageData;
308  for (int y = 0; y < outputDepth.rows; ++y)
309  {
310  for (int x = 0; x < outputDepth.cols; ++x)
311  {
312  uint16_t depthSample = dataShort[y*outputDepth.cols + x];
313  uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
314  outputDepth.at<uint16_t>(y,x) = depthRange;
315  }
316  }
317 
318  if(!outputRGB.empty() && !outputDepth.empty())
319  {
320  double stamp = double(timestamp_ns)/10e8;
321  CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
322  data = SensorData(outputRGB, outputDepth, model, 0, stamp);
323 
324  // pose in OpenGL coordinates
325  float pose_raw[7];
326  HwArCamera_getPose(arSession_, ar_camera, arPose_);
327  HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
328  pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
329  if(pose.isNull())
330  {
331  LOGE("CameraAREngine: Pose is null");
332  }
333  else
334  {
336  this->poseReceived(pose, stamp);
337  // adjust origin
338  if(!getOriginOffset().isNull())
339  {
340  pose = getOriginOffset() * pose;
341  }
342  }
343  }
344  }
345  else
346  {
347  LOGE("CameraAREngine: failed to get rgb image (status=%d %d)", (int)statusRgb, (int)statusDepth);
348  }
349 
350  HwArImage_release(image);
351  HwArImage_release(depthImage);
352  }
353  else
354  {
355  LOGE("Invalid intrinsics!");
356  }
357  }
358 
359  HwArCamera_release(ar_camera);
360  return data;
361 
362 }
363 
364 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::CameraMobile::close
virtual void close()
Definition: CameraMobile.cpp:78
cy
const double cy
rtabmap::CameraMobile::projectionMatrix_
glm::mat4 projectionMatrix_
Definition: CameraMobile.h:140
ret
int ret
rtabmap::CameraMobile::getOriginOffset
const Transform & getOriginOffset() const
Definition: CameraMobile.h:102
fx
const double fx
rtabmap::CameraAREngine::arPose_
HwArPose * arPose_
Definition: CameraAREngine.h:72
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraAREngine::arSessionMutex_
UMutex arSessionMutex_
Definition: CameraAREngine.h:74
rtabmap::CameraMobile::transformed_uvs_
float transformed_uvs_[8]
Definition: CameraMobile.h:141
rtabmap::CameraMobile::uvs_initialized_
bool uvs_initialized_
Definition: CameraMobile.h:142
int64_t
::int64_t int64_t
rtabmap::CameraAREngine::context_
void * context_
Definition: CameraAREngine.h:66
glm::value_ptr
GLM_FUNC_DECL const genType::value_type * value_ptr(genType const &vec)
y
Matrix3f y
rtabmap::CameraMobile::viewMatrix_
glm::mat4 viewMatrix_
Definition: CameraMobile.h:139
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::CameraMobile::deviceTColorCamera_
Transform deviceTColorCamera_
Definition: CameraMobile.h:136
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::rtabmap_world_T_opengl_world
static const rtabmap::Transform rtabmap_world_T_opengl_world(0.0f, 0.0f,-1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f)
uint16_t
::uint16_t uint16_t
data
int data[]
UScopeMutex
Definition: UMutex.h:157
util3d_transforms.h
rtabmap::CameraAREngine::arFrame_
HwArFrame * arFrame_
Definition: CameraAREngine.h:70
rtabmap::CameraMobile::textureId_
GLuint textureId_
Definition: CameraMobile.h:138
rtabmap::opengl_world_T_rtabmap_world
static const rtabmap::Transform opengl_world_T_rtabmap_world(0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f)
rtabmap::CameraAREngine::arCameraIntrinsics_
HwArCameraIntrinsics * arCameraIntrinsics_
Definition: CameraAREngine.h:71
rtabmap::CameraAREngine::env_
void * env_
Definition: CameraAREngine.h:65
UASSERT
#define UASSERT(condition)
x
x
rtabmap::CameraMobile::opticalRotation
static const rtabmap::Transform opticalRotation
Definition: CameraMobile.h:76
OdometryEvent.h
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::CameraMobile::poseReceived
void poseReceived(const Transform &pose, double deviceStamp)
Definition: CameraMobile.cpp:164
rtabmap::CameraMobile::setScreenRotationAndSize
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
Definition: CameraMobile.h:114
ULogger.h
ULogger class and convenient macros.
rtabmap::CameraAREngine::arInstallRequested_
bool arInstallRequested_
Definition: CameraAREngine.h:73
rtabmap::Transform
Definition: Transform.h:41
util2d.h
uint8_t
::uint8_t uint8_t
rtabmap::CameraAREngine::updateDataOnRender
virtual SensorData updateDataOnRender(Transform &pose)
Definition: CameraAREngine.cpp:183
rtabmap::ScreenRotation
ScreenRotation
Definition: util.h:194
BackgroundRenderer_kVerticesView
static const GLfloat BackgroundRenderer_kVerticesView[]
Definition: background_renderer.h:37
env
rtabmap::CameraAREngine::arConfig_
HwArConfig * arConfig_
Definition: CameraAREngine.h:69
BackgroundRenderer::kNumVertices
static constexpr int kNumVertices
Definition: background_renderer.h:56
rtabmap::CameraAREngine::CameraAREngine
CameraAREngine(void *env, void *context, void *activity, bool smoothing=false)
Definition: CameraAREngine.cpp:43
rtabmap::CameraMobile
Definition: CameraMobile.h:71
rtabmap::CameraAREngine::setScreenRotationAndSize
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
Definition: CameraAREngine.cpp:169
rtabmap::CameraAREngine::close
virtual void close()
Definition: CameraAREngine.cpp:133
rtabmap::CameraAREngine::getSerial
virtual std::string getSerial() const
Definition: CameraAREngine.cpp:60
len
size_t len(handle h)
rtabmap::CameraAREngine::arSession_
HwArSession * arSession_
Definition: CameraAREngine.h:68
false
#define false
Definition: ConvertUTF.c:56
cx
const double cx
rtabmap::CameraAREngine::~CameraAREngine
virtual ~CameraAREngine()
Definition: CameraAREngine.cpp:53
rtabmap::glmFromTransform
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:124
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::CameraAREngine::activity_
void * activity_
Definition: CameraAREngine.h:67
UERROR
#define UERROR(...)
stride
Index stride
util.h
trace.model
model
Definition: trace.py:4
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
CameraAREngine.h
int32_t
::int32_t int32_t
fy
const double fy
rtabmap::CameraAREngine::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraAREngine.cpp:65


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