Skip to content
This repository has been archived by the owner on Mar 2, 2018. It is now read-only.

Commit

Permalink
Merge pull request #48 from googlesamples/release-vonmises
Browse files Browse the repository at this point in the history
release-vonmises
  • Loading branch information
jguomoto committed Sep 3, 2015
2 parents 44d10f9 + 9c623f0 commit aa50385
Show file tree
Hide file tree
Showing 14 changed files with 500 additions and 380 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include "tango-area-learning/area_learning_app.h"

namespace {
const int kVersionStringLength = 27;
const int kVersionStringLength = 128;

// This function routes onPoseAvailable callbacks to the application object for
// handling.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,10 @@
#include "tango-augmented-reality/augmented_reality_app.h"

namespace {
const int kVersionStringLength = 27;

// Scale frustum size for closer near clipping plane.
const float kFovScaler = 0.1f;
const int kVersionStringLength = 128;

// Far clipping plane of the AR camera.
const float kArCameraNearClippingPlane = 0.1f;
const float kArCameraFarClippingPlane = 100.0f;

// This function routes onTangoEvent callbacks to the application object for
Expand Down Expand Up @@ -134,17 +132,18 @@ int AugmentedRealityApp::TangoSetupConfig() {
}

// Get TangoCore version string from service.
char tango_core_version[kVersionStringLength];
ret = TangoConfig_getString(
tango_config_, "tango_service_library_version",
const_cast<char*>(tango_core_version_string_.c_str()),
kVersionStringLength);
tango_core_version, kVersionStringLength);
if (ret != TANGO_SUCCESS) {
LOGE(
"AugmentedRealityApp: get tango core version failed with error"
"code: %d",
ret);
return ret;
}
tango_core_version_string_ = tango_core_version;

return ret;
}
Expand Down Expand Up @@ -232,15 +231,18 @@ void AugmentedRealityApp::SetViewPort(int width, int height) {

float image_width = static_cast<float>(color_camera_intrinsics_.width);
float image_height = static_cast<float>(color_camera_intrinsics_.height);
float focus_length = static_cast<float>(color_camera_intrinsics_.fx);
float fx = static_cast<float>(color_camera_intrinsics_.fx);
float fy = static_cast<float>(color_camera_intrinsics_.fy);
float cx = static_cast<float>(color_camera_intrinsics_.cx);
float cy = static_cast<float>(color_camera_intrinsics_.cy);

float image_plane_ratio = image_height / image_width;
float image_plane_distance = 2.0f * focus_length / image_width;
float image_plane_distance = 2.0f * fx / image_width;

glm::mat4 projection_mat_ar = glm::frustum(
-1.0f * kFovScaler, 1.0f * kFovScaler, -image_plane_ratio * kFovScaler,
image_plane_ratio * kFovScaler, image_plane_distance * kFovScaler,
kArCameraFarClippingPlane);
glm::mat4 projection_mat_ar =
tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
image_width, image_height, fx, fy, cx, cy, kArCameraNearClippingPlane,
kArCameraFarClippingPlane);

main_scene_.SetFrustumScale(
glm::vec3(1.0f, image_plane_ratio, image_plane_distance));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#include "tango-motion-tracking/motion_tracking_app.h"

namespace {
const int kVersionStringLength = 27;
const int kVersionStringLength = 128;

// This function routes onPoseAvailable callbacks to the application object for
// handling.
Expand Down Expand Up @@ -92,17 +92,18 @@ int MotiongTrackingApp::TangoSetupConfig(bool is_atuo_recovery) {
}

// Get TangoCore version string from service.
char tango_core_version[kVersionStringLength];
ret = TangoConfig_getString(
tango_config_, "tango_service_library_version",
const_cast<char*>(tango_core_version_string_.c_str()),
kVersionStringLength);
tango_core_version, kVersionStringLength);
if (ret != TANGO_SUCCESS) {
LOGE(
"MotiongTrackingApp: get tango core version failed with error"
"code: %d",
ret);
return ret;
}
tango_core_version_string_ = tango_core_version;

return ret;
}
Expand Down Expand Up @@ -204,4 +205,4 @@ void MotiongTrackingApp::OnTouchEvent(int touch_count,
main_scene_.OnTouchEvent(touch_count, event, x0, y0, x1, y1);
}

} // namespace tango_motion_tracking
} // namespace tango_motion_tracking
11 changes: 6 additions & 5 deletions point-cloud-jni-example/app/src/main/jni/point_cloud_app.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#include "tango-point-cloud/point_cloud_app.h"

namespace {
const int kVersionStringLength = 27;
const int kVersionStringLength = 128;

// This function routes onXYZijAvailable callbacks to the application object for
// handling.
Expand Down Expand Up @@ -119,17 +119,18 @@ int PointCloudApp::TangoSetupConfig(bool is_atuo_recovery) {
}

// Get TangoCore version string from service.
char tango_core_version[kVersionStringLength];
ret = TangoConfig_getString(
tango_config_, "tango_service_library_version",
const_cast<char*>(tango_core_version_string_.c_str()),
kVersionStringLength);
tango_core_version, kVersionStringLength);
if (ret != TANGO_SUCCESS) {
LOGE(
"PointCloudApp: get tango core version failed with error"
"code: %d",
ret);
return ret;
}
tango_core_version_string_ = tango_core_version;

return ret;
}
Expand Down Expand Up @@ -352,15 +353,15 @@ TangoErrorType PointCloudApp::UpdateExtrinsics() {

// Get color camera with respect to imu transformation matrix.
frame_pair.base = TANGO_COORDINATE_FRAME_IMU;
frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_COLOR;
frame_pair.target = TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
if (ret != TANGO_SUCCESS) {
LOGE(
"PointCloudApp: Failed to get transform between the color camera frame "
"and device frames");
return ret;
}
pose_data_.SetImuTColorCamera(pose_data_.GetMatrixFromPose(pose_data));
pose_data_.SetImuTDepthCamera(pose_data_.GetMatrixFromPose(pose_data));
return ret;
}

Expand Down
10 changes: 4 additions & 6 deletions point-cloud-jni-example/app/src/main/jni/pose_data.cc
Original file line number Diff line number Diff line change
Expand Up @@ -56,19 +56,17 @@ glm::mat4 PoseData::GetExtrinsicsAppliedOpenGLWorldFrame(
// opengl_world_T_start_service *
// start_service_T_device *
// device_T_imu *
// imu_T_color_camera *
// color_camera_T_opengl_camera;
// Note that color camera and depth camera are the same hardware, they share
// the same frame.
// imu_T_depth_camera *
// depth_camera_T_opengl_camera;
//
// More information about frame transformation can be found here:
// Frame of reference:
// https://developers.google.com/project-tango/overview/frames-of-reference
// Coordinate System Conventions:
// https://developers.google.com/project-tango/overview/coordinate-systems
return tango_gl::conversions::opengl_world_T_tango_world() * pose_matrix *
glm::inverse(GetImuTDevice()) * GetImuTColorCamera() *
tango_gl::conversions::color_camera_T_opengl_camera();
glm::inverse(GetImuTDevice()) * GetImuTDepthCamera() *
tango_gl::conversions::depth_camera_T_opengl_camera();
}

glm::mat4 PoseData::GetMatrixFromPose(const TangoPoseData& pose) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,13 @@ class PoseData {
imu_T_device_ = imu_T_device;
}

// @return: color camera frame with respect to IMU frame.
glm::mat4 GetImuTColorCamera() { return imu_T_color_camera_; }
// @return: depth camera frame with respect to IMU frame.
glm::mat4 GetImuTDepthCamera() { return imu_T_depth_camera_; }

// Set color camera frame with respect to IMU frame matrix.
// Set depth camera frame with respect to IMU frame matrix.
// @param: imu_T_color_camera, imu_T_color_camera_ matrix.
void SetImuTColorCamera(const glm::mat4& imu_T_color_camera) {
imu_T_color_camera_ = imu_T_color_camera;
void SetImuTDepthCamera(const glm::mat4& imu_T_depth_camera) {
imu_T_depth_camera_ = imu_T_depth_camera;
}

// Get pose transformation in OpenGL coordinate system. This function also
Expand Down Expand Up @@ -92,7 +92,7 @@ class PoseData {
glm::mat4 imu_T_device_;

// Color camera frame with respect to IMU frame.
glm::mat4 imu_T_color_camera_;
glm::mat4 imu_T_depth_camera_;

// Pose data of current frame.
TangoPoseData cur_pose_;
Expand Down
32 changes: 18 additions & 14 deletions rgb-depth-sync-example/app/src/main/jni/depth_image.cc
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,14 @@ DepthImage::~DepthImage() {
// the depth image into a texture.
// This function also takes care of swapping the Render buffer and shared buffer
// if there is new point cloud data available.
// Ci_T_Cj: 'i' represents the color camera frame's timestamp, 'j' represents
// the depth camera timestamp. The transformation is the camera's frame on
// timestamp j with respect the camera's frame on timestamp i.
// This transform can move a point from depth camera frame to color camera
// frame.
// color_t1_T__depth_t0: 't1' represents the color camera frame's timestamp,
// 't0'
// represents
// the depth camera timestamp. The transformation is the depth camera's frame on
// timestamp t0 with respect the rgb camera's frame on timestamp t1.
void DepthImage::UpdateAndUpsampleDepth(
glm::mat4& Ci_T_Cj, const std::vector<float>& render_point_cloud_buffer) {
glm::mat4& color_t1_T_depth_t0,
const std::vector<float>& render_point_cloud_buffer) {

int depth_image_width = rgb_camera_intrinsics_.width;
int depth_image_height = rgb_camera_intrinsics_.height;
Expand All @@ -60,29 +61,32 @@ void DepthImage::UpdateAndUpsampleDepth(
float y = render_point_cloud_buffer[i + 1];
float z = render_point_cloud_buffer[i + 2];

// Cj_point is the point in camera frame on timestamp j (depth timestamp).
glm::vec4 Cj_point = glm::vec4(x, y, z, 1.0);
// Ci_point is the point in camera frame on timestamp i (color timestamp).
glm::vec4 Ci_point = Ci_T_Cj * Cj_point;
// depth_t0_point is the point in depth camera frame on timestamp t0.
// (depth image timestamp).
glm::vec4 depth_t0_point = glm::vec4(x, y, z, 1.0);

// color_t1_point is the point in camera frame on timestamp t1.
// (color image timestamp).
glm::vec4 color_t1_point = color_t1_T_depth_t0 * depth_t0_point;

int pixel_x, pixel_y;
// get the coordinate on image plane.
pixel_x = static_cast<int>((rgb_camera_intrinsics_.fx) *
(Ci_point.x / Ci_point.z) +
(color_t1_point.x / color_t1_point.z) +
rgb_camera_intrinsics_.cx);

pixel_y = static_cast<int>((rgb_camera_intrinsics_.fy) *
(Ci_point.y / Ci_point.z) +
(color_t1_point.y / color_t1_point.z) +
rgb_camera_intrinsics_.cy);

// Color value is the GL_LUMINANCE value used for displaying the depth
// image.
// We can query for depth value in mm from grayscale image buffer by
// getting a `pixel_value` at (pixel_x,pixel_y) and calculating
// pixel_value * (kMaxDepthDistance / USHRT_MAX)
float depth_value = Ci_point.z;
float depth_value = color_t1_point.z;
uint16_t grayscale_value =
(Ci_point.z * kMeterToMillimeter) * USHRT_MAX / kMaxDepthDistance;
(color_t1_point.z * kMeterToMillimeter) * USHRT_MAX / kMaxDepthDistance;

UpSampleDepthAroundPoint(grayscale_value, depth_value, pixel_x, pixel_y,
&grayscale_display_buffer_, &depth_map_buffer_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,25 +23,26 @@
#include <mutex>
namespace rgb_depth_sync {

// DepthImage is a class which projects point cloud on to a drawable
// DepthImage is a class which projects point cloud on to a color camera's
// image plane.
class DepthImage {
public:
explicit DepthImage();
~DepthImage();

// Update the depth texture with current transformation and current depth map.
// @param Ci_T_Cj: The transformation between the camera frame on timestamp i
// (color camera timestamp) and the camera frame on timestamp j (depth
// @param color_t1_T_depth_t0: The transformation between the color camera frame on timestamp i
// (color camera timestamp) and the depth camera frame on timestamp j (depth
// camera timestamp)
// To convert a point in the camera frame on timestamp j to the camera frame
// on timestamp i, you could do:
// Ci_point = Ci_T_Cj * Cj_point;
// To convert a point in the depth camera frame on timestamp t0 to the color
// camera frame
// on timestamp t1, you could do:
// color_t1_point = color_t1_T_depth_t0 * depth_t0_point;
//
// @param render_point_cloud_buffer: This contains the latest point cloud data
// that gets projected on to the image plane and fills up the depth_map_buffer
void UpdateAndUpsampleDepth(
glm::mat4& Ci_T_Cj, const std::vector<float>& render_point_cloud_buffer);
glm::mat4& color_t1_T_depth_t0, const std::vector<float>& render_point_cloud_buffer);

// Returns the depth texture id.
GLuint GetTextureId() const { return texture_id_; }
Expand Down Expand Up @@ -96,6 +97,8 @@ class DepthImage {
// depth camera are the same hardware on the device.
TangoCameraIntrinsics rgb_camera_intrinsics_;

// Transform between Color camera and Depth Camera.
glm::mat4 depth_camera_T_color_camera_;
};
} // namespace rgb_depth_sync

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,13 +116,11 @@ class SynchronizationApplication {
// this example.
TangoConfig tango_config_;

// Depth Extrinsics
glm::mat4 Dj_T_Cj_;
glm::mat4 Cj_T_Dj_;
// Extrinsic transformation of color frame wrt device frame.
glm::mat4 device_T_color_;

// Color Extrinsics
glm::mat4 Di_T_Ci_;
glm::mat4 Ci_T_Di_;
// Extrinsic transformation of depth frame wrt device frame.
glm::mat4 device_T_depth_;

// OpenGL to Start of Service
glm::mat4 OW_T_SS_;
Expand Down
Loading

0 comments on commit aa50385

Please sign in to comment.