├── .editorconfig ├── .gitattributes ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── CONTRIBUTING ├── LICENSE ├── README.md ├── Rebracer.xml ├── res ├── cube_60mm.dat ├── cube_64mm.artoolkitplus.dat ├── cube_64mm.dat └── detector_params.yaml ├── src ├── align_nvm_pose │ └── align_nvm_pose_cli.cpp ├── aruco │ ├── aruco_estimate_pose_cli.cpp │ ├── aruco_pose_estimator.cpp │ ├── aruco_pose_estimator.h │ ├── cube_fiducial.cpp │ ├── cube_fiducial.h │ ├── single_marker_fiducial.cpp │ └── single_marker_fiducial.h ├── calibrated_posed_depth_camera.h ├── camera_math.cuh ├── control_widget.cpp ├── control_widget.h ├── depth_fusion.cpp ├── depth_processor.cu ├── depth_processor.h ├── detector_params.yml ├── fuse.cu ├── fuse.h ├── fuse_depth │ └── fuse_depth_cli.cpp ├── icp_least_squares_data.cpp ├── icp_least_squares_data.h ├── input_buffer.cpp ├── input_buffer.h ├── interpolate_depth_pose │ └── interpolate_depth_pose_cli.cpp ├── main_controller.cpp ├── main_controller.h ├── main_widget.cpp ├── main_widget.h ├── marching_cubes.cpp ├── marching_cubes.h ├── multi_static_camera_gl_state.cpp ├── multi_static_camera_gl_state.h ├── multi_static_camera_pipeline.cpp ├── multi_static_camera_pipeline.h ├── pipeline_data_type.h ├── pose_estimation_method.h ├── pose_frame.h ├── pose_utils.cpp ├── pose_utils.h ├── projective_point_plane_icp.cu ├── projective_point_plane_icp.h ├── raycast.cu ├── raycast.h ├── raycast_volume │ └── raycast_volume_cli.cpp ├── regular_grid_fusion_pipeline.cpp ├── regular_grid_fusion_pipeline.h ├── regular_grid_tsdf.cu ├── regular_grid_tsdf.h ├── rgbd_camera_parameters.cpp ├── rgbd_camera_parameters.h ├── rgbd_input.cpp ├── rgbd_input.h ├── shaders │ ├── draw_color.fs.glsl │ ├── draw_color_discard_transparent.fs.glsl │ ├── draw_single_color.fs.glsl │ ├── draw_texture.fs.glsl │ ├── draw_world_normal.fs.glsl │ ├── position_color.vs.glsl │ ├── position_normal.vs.glsl │ ├── position_only.vs.glsl │ ├── position_texcoord.vs.glsl │ ├── unproject_point_cloud.vs.glsl │ ├── unproject_point_cloud_textured.vs.glsl │ └── visualize_texcoords.fs.glsl ├── single_moving_camera_gl_state.cpp ├── single_moving_camera_gl_state.h ├── tsdf.h └── visualize_camera_path │ ├── main_widget.cpp │ ├── main_widget.h │ └── visualize_camera_path_cli.cpp ├── src_deprecated ├── artoolkit_pose_estimator.cpp ├── artoolkit_pose_estimator.h ├── artoolkitplus_pose_estimator.cpp └── artoolkitplus_pose_estimator.h └── third_party └── bitmask_operators.hpp /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*.{c,cpp,h,cc,cu}] 4 | indent_style = space 5 | indent_size = 2 6 | tab_width = 2 7 | insert_final_newline = true 8 | trim_trailing_whitespace = true 9 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | * text=auto 2 | 3 | # These files are text and should be normalized (convert crlf --> lf) 4 | *.h text 5 | *.inl text 6 | *.cpp text 7 | *.cu text 8 | *.h text 9 | # Qt project files 10 | *.pro text 11 | 12 | # Scripts 13 | *.bat text 14 | *.fsx text 15 | *.ps1 text 16 | *.cmd text 17 | 18 | *.cs text diff=csharp 19 | *.xaml text 20 | *.csproj text 21 | *.sln text 22 | *.tt text 23 | *.msbuild text 24 | *.md text 25 | 26 | # Images should be treated as binary 27 | # (binary is a macro for -text -diff) 28 | *.png binary 29 | *.jpg binary 30 | *.jpeg binary 31 | *.pfm binary 32 | *.pgm binary 33 | *.ppm binary 34 | 35 | *.sdf binary 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | debug 2 | release 3 | lib 4 | ipch 5 | *.opensdf 6 | *.pdb 7 | *.sdf 8 | *.sln 9 | *.suo 10 | *.vcxproj 11 | *.vcxproj.* 12 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/pystring"] 2 | path = third_party/pystring 3 | url = https://github.com/imageworks/pystring 4 | [submodule "third_party/gflags"] 5 | path = third_party/gflags 6 | url = https://github.com/gflags/gflags 7 | [submodule "third_party/cpp11-range"] 8 | path = third_party/cpp11-range 9 | url = https://github.com/harrism/cpp11-range 10 | [submodule "third_party/Eigen"] 11 | path = third_party/Eigen 12 | url = https://github.com/RLovelett/eigen 13 | -------------------------------------------------------------------------------- /CONTRIBUTING: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/google/depth_fusion/6c8dc4ee88e8afa1c232f5e17391fc95c41b3253/CONTRIBUTING -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## DepthFusion 2 | 3 | This is not an official Google product. 4 | 5 | ## Overview 6 | 7 | **DepthFusion** is an open source software library for reconstructing 3D surfaces (meshes) from depth data produced by commercial off-the-shelf depth cameras such as Microsoft Kinect, Asus Xtion Pro, and Intel RealSense. 8 | 9 | It is at its core an implementation of "A Volumetric Method for Building Complex Models from Range Images" by Curless and Levoy [SIGGRAPH 1996]. Camera pose can be determined using the frame-to-model technique popularized by KinectFusion [Newcombe et al. 2011], or using fiducial markers from OpenCV's ArUco contrib module. 10 | 11 | It runs on real-time on high-end desktop GPUs. 12 | 13 | ## Code structure 14 | 15 | C++ and CUDA code are in **src**. 16 | GLSL shaders for the visualization are in **src/shaders**. 17 | 18 | ## Dependencies 19 | 20 | * Qt 5.5 21 | * CUDA 7.5 (8.0 on Windows for VS2015/C++11 support). 22 | * libcgt (see below) and its transitive dependencies. 23 | * GLEW 24 | * OpenCV 3.0 25 | 26 | ## Build instructions (CMake) 27 | 0. Get [https://github.com/jiawen/libcgt](https://github.com/jiawen/libcgt "libcgt"). 28 | 1. Put libcgt the same level as depth_fusion (they should be siblings). 29 | 2. mkdir build 30 | 3. cd build 31 | 4. cmake-gui .. 32 | 33 | ## License 34 | 35 | Apache 2.0. 36 | -------------------------------------------------------------------------------- /res/cube_60mm.dat: -------------------------------------------------------------------------------- 1 | #the number of patterns to be recognized 2 | 6 3 | 4 | #marker 1 5 | 00 6 | 40.0 7 | 1.0000 0.0000 0.0000 0.0000 8 | 0.0000 1.0000 0.0000 0.0000 9 | 0.0000 0.0000 1.0000 0.0000 10 | 11 | #marker 2 12 | 01 13 | 40.0 14 | 1.0000 0.0000 0.0000 0.0000 15 | 0.0000 0.0000 1.0000 30.0000 16 | 0.0000 -1.0000 0.0000 -30.0000 17 | 18 | #marker 3 19 | 02 20 | 40.0 21 | 0.0000 0.0000 1.0000 30.0000 22 | 0.0000 1.0000 0.0000 0.0000 23 | -1.0000 0.0000 0.0000 -30.0000 24 | 25 | #marker 4 26 | 03 27 | 40.0 28 | 1.0000 0.0000 0.0000 0.0000 29 | 0.0000 -1.0000 0.0000 0.0000 30 | 0.0000 0.0000 -1.0000 -60.0000 31 | 32 | #marker 5 33 | 04 34 | 40.0 35 | 1.0000 0.0000 0.0000 0.0000 36 | 0.0000 0.0000 -1.0000 -30.0000 37 | 0.0000 1.0000 0.0000 -30.0000 38 | 39 | #marker 6 40 | 05 41 | 40.0 42 | 0.0000 0.0000 -1.0000 -30.0000 43 | 0.0000 1.0000 0.0000 0.0000 44 | 1.0000 0.0000 0.0000 -30.0000 45 | -------------------------------------------------------------------------------- /res/cube_64mm.artoolkitplus.dat: -------------------------------------------------------------------------------- 1 | #the number of patterns to be recognized 2 | 6 3 | 4 | #marker 0 5 | 00 6 | 42.66666666666 7 | 0.0 0.0 8 | 1.0000 0.0000 0.0000 0.0000 9 | 0.0000 1.0000 0.0000 0.0000 10 | 0.0000 0.0000 1.0000 0.0000 11 | 12 | #marker 1 13 | 01 14 | 42.66666666666 15 | 0.0 0.0 16 | 1.0000 0.0000 0.0000 0.0000 17 | 0.0000 0.0000 1.0000 32.0000 18 | 0.0000 -1.0000 0.0000 -32.0000 19 | 20 | #marker 2 21 | 02 22 | 42.66666666666 23 | 0.0 0.0 24 | 0.0000 0.0000 1.0000 32.0000 25 | 0.0000 1.0000 0.0000 0.0000 26 | 1.0000 0.0000 0.0000 -32.0000 27 | 28 | #marker 3 29 | 03 30 | 42.66666666666 31 | 0.0 0.0 32 | 1.0000 0.0000 0.0000 0.0000 33 | 0.0000 -1.0000 0.0000 0.0000 34 | 0.0000 0.0000 -1.0000 -64.0000 35 | 36 | #marker 4 37 | 04 38 | 42.66666666666 39 | 0.0 0.0 40 | 1.0000 0.0000 0.0000 0.0000 41 | 0.0000 0.0000 -1.0000 -32.0000 42 | 0.0000 1.0000 0.0000 -32.0000 43 | 44 | #marker 5 45 | 05 46 | 42.66666666666 47 | 0.0 0.0 48 | 0.0000 0.0000 -1.0000 -32.0000 49 | 0.0000 1.0000 0.0000 0.0000 50 | 1.0000 0.0000 0.0000 -32.0000 51 | -------------------------------------------------------------------------------- /res/cube_64mm.dat: -------------------------------------------------------------------------------- 1 | #the number of patterns to be recognized 2 | 6 3 | 4 | #marker 1 5 | 00 6 | 42.66666666666 7 | 1.0000 0.0000 0.0000 0.0000 8 | 0.0000 1.0000 0.0000 0.0000 9 | 0.0000 0.0000 1.0000 0.0000 10 | 11 | #marker 2 12 | 01 13 | 42.66666666666 14 | 1.0000 0.0000 0.0000 0.0000 15 | 0.0000 0.0000 1.0000 32.0000 16 | 0.0000 -1.0000 0.0000 -32.0000 17 | 18 | #marker 3 19 | 02 20 | 42.66666666666 21 | 0.0000 0.0000 1.0000 32.0000 22 | 0.0000 1.0000 0.0000 0.0000 23 | -1.0000 0.0000 0.0000 -32.0000 24 | 25 | #marker 4 26 | 03 27 | 42.66666666666 28 | 1.0000 0.0000 0.0000 0.0000 29 | 0.0000 -1.0000 0.0000 0.0000 30 | 0.0000 0.0000 -1.0000 -64.0000 31 | 32 | #marker 5 33 | 04 34 | 42.66666666666 35 | 1.0000 0.0000 0.0000 0.0000 36 | 0.0000 0.0000 -1.0000 -32.0000 37 | 0.0000 1.0000 0.0000 -32.0000 38 | 39 | #marker 6 40 | 05 41 | 42.66666666666 42 | 0.0000 0.0000 -1.0000 -32.0000 43 | 0.0000 1.0000 0.0000 0.0000 44 | 1.0000 0.0000 0.0000 -32.0000 45 | -------------------------------------------------------------------------------- /res/detector_params.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | nmarkers: 6 3 | adaptiveThreshWinSize: 21 4 | #adaptiveThreshWinSizeMin: 5 5 | #adaptiveThreshWinSizeMax: 5 6 | adaptiveThreshWinSizeMin: 3 7 | adaptiveThreshWinSizeMax: 64 8 | adaptiveThreshWinSizeStep: 1 9 | adaptiveThreshConstant: 7 10 | minMarkerPerimeterRate: 0.03 11 | maxMarkerPerimeterRate: 4.0 12 | polygonalApproxAccuracyRate: 0.05 13 | minCornerDistance: 10.0 14 | minDistanceToBorder: 3 15 | minMarkerDistance: 10.0 16 | doCornerRefinement: true 17 | cornerRefinementWinSize: 5 18 | cornerRefinementMaxIterations: 30 19 | cornerRefinementMinAccuracy: 0.1 20 | markerBorderBits: 1 21 | perspectiveRemovePixelPerCell: 8 22 | perspectiveRemoveIgnoredMarginPerCell: 0.13 23 | maxErroneousBitsInBorderRate: 0.04 24 | -------------------------------------------------------------------------------- /src/aruco/aruco_estimate_pose_cli.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "libcgt/core/vecmath/Vector2i.h" 6 | #include "libcgt/core/vecmath/EuclideanTransform.h" 7 | #include "libcgt/camera_wrappers/PoseStream.h" 8 | 9 | #include "src/rgbd_camera_parameters.h" 10 | #include "src/rgbd_input.h" 11 | #include "src/input_buffer.h" 12 | 13 | #include "src/aruco/aruco_pose_estimator.h" 14 | #include "src/aruco/cube_fiducial.h" 15 | #include "src/aruco/single_marker_fiducial.h" 16 | 17 | using libcgt::camera_wrappers::PoseInputStream; 18 | using libcgt::camera_wrappers::PoseOutputStream; 19 | using libcgt::camera_wrappers::PoseStreamFormat; 20 | using libcgt::camera_wrappers::PoseStreamMetadata; 21 | using libcgt::camera_wrappers::PoseStreamTransformDirection; 22 | using libcgt::camera_wrappers::PoseStreamUnits; 23 | using libcgt::core::vecmath::EuclideanTransform; 24 | 25 | DEFINE_string(input_file, "", "Input .rgbd file."); 26 | 27 | DEFINE_string(calibration_dir, "", 28 | "calibration directory for the RGBD camera."); 29 | 30 | DEFINE_string(output_file, "", "output .pose file."); 31 | 32 | DEFINE_bool(collect_perf, false, "Collect performance statistics."); 33 | 34 | const char* kArucoDetectorParamsFilename = "../res/detector_params.yaml"; 35 | 36 | int main(int argc, char* argv[]) { 37 | gflags::ParseCommandLineFlags(&argc, &argv, true); 38 | if (FLAGS_input_file == "") { 39 | printf("input_file is required.\n"); 40 | return 1; 41 | } 42 | if (FLAGS_calibration_dir == "") { 43 | printf("calibration_dir is required.\n"); 44 | return 1; 45 | } 46 | if (FLAGS_output_file == "") { 47 | printf("output_file is required.\n"); 48 | return 1; 49 | } 50 | 51 | // TODO: tweak this so that it can fail. 52 | RGBDCameraParameters camera_params; 53 | bool ok = LoadRGBDCameraParameters(FLAGS_calibration_dir, &camera_params); 54 | if (!ok) { 55 | fprintf(stderr, "Error loading RGBD camera parameters from %s.\n", 56 | FLAGS_calibration_dir.c_str()); 57 | return 1; 58 | } 59 | 60 | bool rgb_updated; 61 | bool depth_updated; 62 | RgbdInput rgbd_input(RgbdInput::InputType::FILE, FLAGS_input_file.c_str()); 63 | 64 | InputBuffer input_buffer( 65 | camera_params.color.resolution, 66 | camera_params.depth.resolution); 67 | 68 | // TODO: let the user specify which fiducial. 69 | float side_length = SingleMarkerFiducial::kDefaultSideLength; 70 | int single_marker_id = 3; 71 | printf("Using single marker fiducial with side length %f m, id = %d\n", 72 | side_length, single_marker_id); 73 | SingleMarkerFiducial fiducial(side_length, single_marker_id); 74 | ArucoPoseEstimator pose_estimator( 75 | fiducial, 76 | camera_params.color, 77 | kArucoDetectorParamsFilename); 78 | 79 | PoseStreamMetadata metadata; 80 | metadata.format = PoseStreamFormat::ROTATION_MATRIX_3X3_COL_MAJOR_AND_TRANSLATION_VECTOR_FLOAT; 81 | metadata.units = PoseStreamUnits::METERS; 82 | metadata.direction = PoseStreamTransformDirection::CAMERA_FROM_WORLD; 83 | 84 | PoseOutputStream output_stream(metadata, FLAGS_output_file.c_str()); 85 | 86 | // Read initial frame. 87 | rgbd_input.read(&input_buffer, &rgb_updated, &depth_updated); 88 | while (rgb_updated || depth_updated) { 89 | if (rgb_updated) { 90 | printf("Processing frame. idx = %d, timestamp = %lld\n", 91 | input_buffer.color_frame_index, 92 | input_buffer.color_timestamp_ns); 93 | auto result = pose_estimator.EstimatePose(input_buffer.color_bgr_ydown); 94 | if (result.valid) { 95 | auto cfw = inverse(result.world_from_camera); 96 | output_stream.write( 97 | input_buffer.color_frame_index, 98 | input_buffer.color_timestamp_ns, 99 | cfw.rotation, 100 | cfw.translation 101 | ); 102 | } else { 103 | printf("Failed to find pose.\n"); 104 | } 105 | } 106 | 107 | // Read next frame. 108 | rgbd_input.read(&input_buffer, &rgb_updated, &depth_updated); 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /src/aruco/aruco_pose_estimator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef ARUCO_POSE_ESTIMATOR_H 15 | #define ARUCO_POSE_ESTIMATOR_H 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "libcgt/core/common/Array2D.h" 22 | #include "libcgt/core/common/BasicTypes.h" 23 | #include "libcgt/core/vecmath/EuclideanTransform.h" 24 | 25 | struct CameraParameters; 26 | 27 | class ArucoPoseEstimator { 28 | public: 29 | 30 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 31 | 32 | // Pose estimate. 33 | // 34 | // If valid = true: 35 | // 36 | // camera_from_board will be a rigid transform mapping camera <-- board. 37 | // 38 | // camera_from_board_rotation and camera_from_board_translation represents 39 | // the same transformation but as as rotation and translation vectors. 40 | // 41 | // Coordinate conventions: 42 | // 43 | // Aruco stores the board's coordinates using OpenGL conventions. 44 | // 45 | // camera_from_board is a fully-OpenGL transformation, mapping a GL board 46 | // to a GL camera (y up, z out of screen for both). 47 | // 48 | // camera_from_board_rotation and camera_from_board_translation maps from 49 | // a GL board (y up, z out of screen) to a CV camera 50 | // (y down, z into screen). 51 | // 52 | // Therefore, there is a 180 degree rotation about the x axis between the 53 | // two representations. 54 | struct Result { 55 | bool valid = false; 56 | 57 | EuclideanTransform world_from_camera; 58 | 59 | cv::Vec3d camera_from_board_rotation; 60 | cv::Vec3d camera_from_board_translation; 61 | }; 62 | 63 | // TODO: pass in a cv::aruco::DetectorParameters object instead of a 64 | // filename. 65 | ArucoPoseEstimator(const cv::aruco::Board& fiducial, 66 | const CameraParameters& params, 67 | const std::string& detector_params_filename); 68 | 69 | // Estimate the camera pose given a color image input. Input should have 70 | // PixelFormat "bgr". 71 | // 72 | // [Optional] If vis is not null, also draws a visualization of the pose 73 | // estimate into vis. vis should have PixelFormat BGR. 74 | // 75 | // In both cases, the y axis points down (OpenCV convention). 76 | Result EstimatePose(Array2DReadView input, 77 | Array2DWriteView vis = Array2DWriteView()) const; 78 | 79 | private: 80 | 81 | struct Detection { 82 | std::vector< int > ids; 83 | std::vector< std::vector< cv::Point2f > > corners; 84 | std::vector< std::vector< cv::Point2f > > rejected; 85 | }; 86 | 87 | Detection Detect(cv::Mat image) const; 88 | 89 | // Refine the markers returned by Detect(). 90 | // image must be the same as the one used in Detect(). 91 | // detection is modified in place. 92 | void Refine(cv::Mat image, Detection* detection) const; 93 | 94 | // Estimate the camera pose from detected and optionally refined markers. 95 | // The Detection may be invalid if there are not enough corners. 96 | Result EstimatePose(const Detection& detection) const; 97 | 98 | cv::Mat camera_intrinsics_; 99 | cv::Mat camera_dist_coeffs_; 100 | 101 | cv::aruco::Board board_; 102 | cv::aruco::DetectorParameters detector_params_; 103 | 104 | // Rotation matrix about the x-axis by 180 degrees. 105 | const Matrix4f rot_x_180_; 106 | 107 | // For visualization. 108 | float axis_length_meters_; 109 | 110 | static void VisualizeDetections(const Detection& detection, 111 | bool show_rejected, Array2DWriteView output); 112 | 113 | void VisualizePoseEstimate(const Result& pose_estimate, 114 | Array2DWriteView output) const; 115 | }; 116 | 117 | bool ReadDetectorParameters(const std::string& filename, 118 | cv::aruco::DetectorParameters& params); 119 | 120 | #endif // ARUCO_POSE_ESTIMATOR_H 121 | -------------------------------------------------------------------------------- /src/aruco/cube_fiducial.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "cube_fiducial.h" 15 | 16 | #include "libcgt/core/common/Array2D.h" 17 | #include "libcgt/core/common/ArrayUtils.h" 18 | #include "libcgt/core/geometry/BoxUtils.h" 19 | #include "libcgt/core/vecmath/Vector2i.h" 20 | #include "libcgt/opencv_interop/ArrayUtils.h" 21 | #include "libcgt/opencv_interop/VecmathUtils.h" 22 | 23 | using cv::aruco::Board; 24 | using cv::Point3f; 25 | using libcgt::core::arrayutils::flipYInPlace; 26 | using libcgt::core::geometry::corners; 27 | using libcgt::core::geometry::makeBox; 28 | using libcgt::core::geometry::solidBoxTriangleListIndices; 29 | using libcgt::opencv_interop::array2DViewAsCvMat; 30 | using libcgt::opencv_interop::toCVPoint; 31 | using std::vector; 32 | 33 | CubeFiducial::CubeFiducial(float side_length) : 34 | CubeFiducial(makeBox(Vector3f{0}, side_length)) { 35 | } 36 | 37 | CubeFiducial::CubeFiducial(const Box3f& box) : 38 | box_(box) { 39 | objPoints.resize(kNumMarkers, vector(kNumPointsPerMarker)); 40 | dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); 41 | ids = {0, 1, 2, 3, 4, 5}; 42 | 43 | // Vertex positions in hypercube order. 44 | // The "code box" has the same center but is only 3/4 the size. 45 | // Each face of the code box is 6x6 bits: 4x4 bits for the actual code, with 46 | // a 1 bit black border. 47 | Box3f code_box = makeBox(box.center(), 0.75f * box.size.x); 48 | Array1D box_positions = corners(code_box); 49 | // Each face, looked head on, in clockwise order, from the top left. 50 | const int aruco_cube_face_indices[kNumMarkers][kNumPointsPerMarker] = { 51 | {2, 6, 4, 0}, // left 52 | {7, 3, 1, 5}, // right 53 | {4, 5, 1, 0}, // bottom 54 | {2, 3, 7, 6}, // top 55 | {3, 2, 0, 1}, // back 56 | {6, 7, 5, 4} // front 57 | }; 58 | 59 | for (int i = 0; i < kNumMarkers; ++i) { 60 | for (int j = 0; j < 4; ++j) { 61 | Vector3f p = box_positions[aruco_cube_face_indices[i][j]].xyz; 62 | objPoints[i][j] = toCVPoint(p); 63 | } 64 | } 65 | } 66 | 67 | CubeFiducialDrawable::CubeFiducialDrawable(const CubeFiducial& fiducial, 68 | int face_size_texels) : 69 | GLDrawable(GLPrimitiveType::TRIANGLES, calculator()), 70 | texture_({CubeFiducial::kNumMarkers * face_size_texels, face_size_texels}, 71 | GLImageInternalFormat::R8) { 72 | // Vertex positions in hypercube order. 73 | Array1D box_positions = corners(fiducial.box_); 74 | Array1D gl_indices = solidBoxTriangleListIndices(); 75 | 76 | // Populate GL positions. 77 | { 78 | auto mb = mapAttribute(0); 79 | Array1DWriteView positions = mb.view(); 80 | for (size_t i = 0; i < gl_indices.size(); ++i) { 81 | positions[i] = box_positions[gl_indices[i]].xyz; 82 | } 83 | } 84 | 85 | // Populate GL texture coordinates. 86 | { 87 | float inv_face_size = 1.0f / CubeFiducial::kNumMarkers; 88 | auto mb = mapAttribute(1); 89 | Array1DWriteView texture_coordinates = mb.view(); 90 | for (int i = 0; i < CubeFiducial::kNumMarkers; ++i) { 91 | int b = CubeFiducial::kNumMarkers * i; 92 | texture_coordinates[b ] = Vector2f{i * inv_face_size , 0.0f}; 93 | texture_coordinates[b + 1] = Vector2f{(i + 1) * inv_face_size, 0.0f}; 94 | texture_coordinates[b + 2] = Vector2f{i * inv_face_size , 1.0f}; 95 | 96 | texture_coordinates[b + 3] = Vector2f{i * inv_face_size , 1.0f}; 97 | texture_coordinates[b + 4] = Vector2f{(i + 1) * inv_face_size, 0.0f}; 98 | texture_coordinates[b + 5] = Vector2f{(i + 1) * inv_face_size, 1.0f}; 99 | } 100 | } 101 | 102 | // Set the texture itself. 103 | // 104 | // dict.markerSize = bits per dimension. 105 | // Add a 1 bit border on each side. 106 | constexpr int kBorderBits = 1; 107 | 108 | // Each face comprises an outer border that's white (a ring 1 bit wide), 109 | // a black border around the code (another ring 1 bit wide), followed by 110 | // the 4x4 code. 111 | 112 | // First, fill the texture with white. 113 | texture_.clear(static_cast(255u)); 114 | 115 | // For one face, the code with the inner border occupies 3/4 of the width and 116 | // height. 117 | // Since it's centered, it is offset 1/8 of the way from the corner. 118 | Vector2i code_with_border_size{face_size_texels * 3 / 4}; 119 | Vector2i code_offset{face_size_texels / 8}; 120 | Array2D face_tex_data(code_with_border_size); 121 | cv::Mat cv_view = array2DViewAsCvMat(face_tex_data.writeView()); 122 | 123 | for (int i = 0; i < CubeFiducial::kNumMarkers; ++i) { 124 | int id = fiducial.ids[i]; 125 | // TODO: draw the marker directly using the dictionary byte list instead 126 | // of using dict.drawMarker, draws then resizes. 127 | fiducial.dictionary.drawMarker(id, code_with_border_size.x, cv_view, 128 | kBorderBits); 129 | flipYInPlace(face_tex_data.writeView()); 130 | texture_.set(face_tex_data, GLImageFormat::RED, 131 | {face_size_texels * i + code_offset.x, code_offset.y} /* dstOffset */ ); 132 | } 133 | 134 | texture_.setSwizzleRGBAlpha(GLTexture::SwizzleTarget::RED); 135 | } 136 | 137 | const GLTexture2D& CubeFiducialDrawable::texture() const { 138 | return texture_; 139 | } 140 | 141 | // static 142 | PlanarVertexBufferCalculator CubeFiducialDrawable::calculator() { 143 | constexpr int kNumTriangles = 2 * CubeFiducial::kNumMarkers; 144 | constexpr int nVertices = 3 * kNumTriangles; 145 | PlanarVertexBufferCalculator calculator( nVertices ); 146 | calculator.addAttribute< Vector3f >(); 147 | calculator.addAttribute< Vector2f >(); 148 | return calculator; 149 | } 150 | -------------------------------------------------------------------------------- /src/aruco/cube_fiducial.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef CUBE_FIDUCIAL_H 15 | #define CUBE_FIDUCIAL_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/vecmath/Box3f.h" 20 | #include "libcgt/GL/GL_45/drawables/Drawable.h" 21 | #include "libcgt/GL/GL_45/GLTexture2D.h" 22 | 23 | // A cubical fiducial marker. It is a subclass of cv::aruco::Board, which 24 | // represents the data needed for any kind of 3D fiducial, not just planar 25 | // ones. 26 | class CubeFiducial : public cv::aruco::Board { 27 | public: 28 | 29 | static constexpr int kNumMarkers = 6; 30 | static constexpr int kNumPointsPerMarker = 4; 31 | static constexpr float kDefaultSideLength = 0.064f; // 64 mm. 32 | 33 | CubeFiducial(float side_length = kDefaultSideLength); 34 | CubeFiducial(const Box3f& box); 35 | 36 | // TODO: oriented box? 37 | const Box3f box_; 38 | }; 39 | 40 | // Must be instantiated when an OpenGL context is active. 41 | class CubeFiducialDrawable : public GLDrawable { 42 | public: 43 | 44 | static constexpr int kDefaultFaceSizeTexels = 128; 45 | 46 | CubeFiducialDrawable(const CubeFiducial& fiducial, 47 | int face_size_texels = kDefaultFaceSizeTexels); 48 | 49 | const GLTexture2D& texture() const; 50 | 51 | private: 52 | 53 | static PlanarVertexBufferCalculator calculator(); 54 | GLTexture2D texture_; 55 | }; 56 | 57 | #endif // CUBE_FIDUCIAL_H 58 | -------------------------------------------------------------------------------- /src/aruco/single_marker_fiducial.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "single_marker_fiducial.h" 15 | 16 | #include "libcgt/core/common/Array2D.h" 17 | #include "libcgt/core/common/ArrayUtils.h" 18 | #include "libcgt/core/geometry/BoxUtils.h" 19 | #include "libcgt/core/vecmath/Vector2i.h" 20 | #include "libcgt/opencv_interop/ArrayUtils.h" 21 | #include "libcgt/opencv_interop/VecmathUtils.h" 22 | 23 | using cv::aruco::Board; 24 | using cv::Point3f; 25 | using libcgt::core::arrayutils::flipYInPlace; 26 | using libcgt::core::geometry::corners; 27 | using libcgt::core::geometry::makeBox; 28 | using libcgt::core::geometry::solidBoxTriangleListIndices; 29 | using libcgt::opencv_interop::array2DViewAsCvMat; 30 | using libcgt::opencv_interop::toCVPoint; 31 | using std::vector; 32 | 33 | SingleMarkerFiducial::SingleMarkerFiducial(float side_length, int id) { 34 | objPoints.resize(kNumMarkers, vector(kNumPointsPerMarker)); 35 | dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); 36 | ids = { id }; 37 | 38 | // In clockwise order. 39 | objPoints[0][0] = Point3f{0.0f, 0.0f, 0.0f}; 40 | objPoints[0][1] = Point3f{side_length, 0.0f, 0.0f}; 41 | objPoints[0][2] = Point3f{side_length, 0.0f, side_length}; 42 | objPoints[0][3] = Point3f{0.0f, 0.0f, side_length}; 43 | 44 | #if 0 45 | // Vertex positions in hypercube order. 46 | // The code box has the same center but is only 3/4 the size. 47 | Box3f code_box = makeBox(box.center(), 0.75f * box.size.x); 48 | Array1D box_positions = corners(code_box); 49 | // Each face, looked head on, in clockwise order, from the top left. 50 | const int aruco_cube_face_indices[kNumMarkers][kNumPointsPerMarker] = { 51 | {2, 6, 4, 0}, // left 52 | {7, 3, 1, 5}, // right 53 | {4, 5, 1, 0}, // bottom 54 | {2, 3, 7, 6}, // top 55 | {3, 2, 0, 1}, // back 56 | {6, 7, 5, 4} // front 57 | }; 58 | 59 | for (int i = 0; i < kNumMarkers; ++i) { 60 | for (int j = 0; j < 4; ++j) { 61 | Vector3f p = box_positions[aruco_cube_face_indices[i][j]].xyz; 62 | objPoints[i][j] = toCVPoint(p); 63 | } 64 | } 65 | #endif 66 | } 67 | 68 | #if 0 69 | SingleMarkerFiducialDrawable::SingleMarkerFiducialDrawable(const SingleMarkerFiducial& fiducial, 70 | int face_size_texels) : 71 | GLDrawable(GLPrimitiveType::TRIANGLES, calculator()), 72 | texture_({SingleMarkerFiducial::kNumMarkers * face_size_texels, face_size_texels}, 73 | GLImageInternalFormat::R8) { 74 | // Vertex positions in hypercube order. 75 | Array1D box_positions = corners(fiducial.box_); 76 | Array1D gl_indices = solidBoxTriangleListIndices(); 77 | 78 | // Populate GL positions. 79 | { 80 | auto mb = mapAttribute(0); 81 | Array1DWriteView positions = mb.view(); 82 | for (size_t i = 0; i < gl_indices.size(); ++i) { 83 | positions[i] = box_positions[gl_indices[i]].xyz; 84 | } 85 | } 86 | 87 | // Populate GL texture coordinates. 88 | { 89 | float inv_face_size = 1.0f / SingleMarkerFiducial::kNumMarkers; 90 | auto mb = mapAttribute(1); 91 | Array1DWriteView texture_coordinates = mb.view(); 92 | for (int i = 0; i < SingleMarkerFiducial::kNumMarkers; ++i) { 93 | int b = SingleMarkerFiducial::kNumMarkers * i; 94 | texture_coordinates[b ] = Vector2f{i * inv_face_size , 0.0f}; 95 | texture_coordinates[b + 1] = Vector2f{(i + 1) * inv_face_size, 0.0f}; 96 | texture_coordinates[b + 2] = Vector2f{i * inv_face_size , 1.0f}; 97 | 98 | texture_coordinates[b + 3] = Vector2f{i * inv_face_size , 1.0f}; 99 | texture_coordinates[b + 4] = Vector2f{(i + 1) * inv_face_size, 0.0f}; 100 | texture_coordinates[b + 5] = Vector2f{(i + 1) * inv_face_size, 1.0f}; 101 | } 102 | } 103 | 104 | // Set the texture itself. 105 | // 106 | // dict.markerSize = bits per dimension. 107 | // Add a 1 bit border on each side. 108 | constexpr int kBorderBits = 1; 109 | 110 | // Each face comprises an outer border that's white (a ring 1 bit wide), 111 | // a black border around the code (another ring 1 bit wide), followed by 112 | // the 4x4 code. 113 | 114 | // First, fill the texture with white. 115 | texture_.clear(static_cast(255u)); 116 | 117 | // For one face, the code with the inner border occupies 3/4 of the width and 118 | // height. 119 | // Since it's centered, it is offset 1/8 of the way from the corner. 120 | Vector2i code_with_border_size{face_size_texels * 3 / 4}; 121 | Vector2i code_offset{face_size_texels / 8}; 122 | Array2D face_tex_data(code_with_border_size); 123 | cv::Mat cv_view = array2DViewAsCvMat(face_tex_data.writeView()); 124 | 125 | for (int i = 0; i < SingleMarkerFiducial::kNumMarkers; ++i) { 126 | int id = fiducial.ids[i]; 127 | // TODO: draw the marker directly using the dictionary byte list instead 128 | // of using dict.drawMarker, draws then resizes. 129 | fiducial.dictionary.drawMarker(id, code_with_border_size.x, cv_view, 130 | kBorderBits); 131 | flipYInPlace(face_tex_data.writeView()); 132 | texture_.set(face_tex_data, GLImageFormat::RED, 133 | {face_size_texels * i + code_offset.x, code_offset.y} /* dstOffset */ ); 134 | } 135 | 136 | texture_.setSwizzleRGBAlpha(GLTexture::SwizzleTarget::RED); 137 | } 138 | 139 | const GLTexture2D& SingleMarkerFiducialDrawable::texture() const { 140 | return texture_; 141 | } 142 | 143 | // static 144 | PlanarVertexBufferCalculator SingleMarkerFiducialDrawable::calculator() { 145 | constexpr int kNumTriangles = 2 * SingleMarkerFiducial::kNumMarkers; 146 | constexpr int nVertices = 3 * kNumTriangles; 147 | PlanarVertexBufferCalculator calculator( nVertices ); 148 | calculator.addAttribute< Vector3f >(); 149 | calculator.addAttribute< Vector2f >(); 150 | return calculator; 151 | } 152 | #endif 153 | -------------------------------------------------------------------------------- /src/aruco/single_marker_fiducial.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef SINGLE_MARKER_FIDUCIAL_H 15 | #define SINGLE_MARKER_FIDUCIAL_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/vecmath/Rect2f.h" 20 | #include "libcgt/GL/GL_45/drawables/Drawable.h" 21 | #include "libcgt/GL/GL_45/GLTexture2D.h" 22 | 23 | // A single fiducial marker. It is a subclass of cv::aruco::Board, which 24 | // represents the data needed for any kind of 3D fiducial, not just planar 25 | // ones. 26 | class SingleMarkerFiducial : public cv::aruco::Board { 27 | public: 28 | 29 | static constexpr int kNumMarkers = 1; 30 | static constexpr int kNumPointsPerMarker = 4; 31 | static constexpr float kDefaultSideLength = 0.1524f; // 6 inches, 152.4 mm. 32 | 33 | SingleMarkerFiducial(float side_length = kDefaultSideLength, int id = 0); 34 | 35 | // TODO: oriented rect 36 | const Rect2f rect_; 37 | }; 38 | 39 | // TODO: implement me. 40 | #if 0 41 | // Must be instantiated when an OpenGL context is active. 42 | class SingleMarkerFiducialDrawable : public GLDrawable { 43 | public: 44 | 45 | static constexpr int kDefaultFaceSizeTexels = 128; 46 | 47 | SingleMarkerFiducialDrawable(const SingleMarkerFiducial& fiducial, 48 | int face_size_texels = kDefaultFaceSizeTexels); 49 | 50 | const GLTexture2D& texture() const; 51 | 52 | private: 53 | 54 | static PlanarVertexBufferCalculator calculator(); 55 | GLTexture2D texture_; 56 | }; 57 | #endif 58 | 59 | #endif // SINGLE_MARKER_FIDUCIAL_H 60 | -------------------------------------------------------------------------------- /src/calibrated_posed_depth_camera.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef CALIBRATED_POSED_DEPTH_CAMERA_H 15 | #define CALIBRATED_POSED_DEPTH_CAMERA_H 16 | 17 | #include 18 | 19 | #include "libcgt/cuda/float4x4.h" 20 | #include "libcgt/cuda/KernelArray2D.h" 21 | 22 | struct CalibratedPosedDepthCamera { 23 | float4 flpp; 24 | float2 depth_min_max; 25 | float4x4 camera_from_world; 26 | }; 27 | 28 | #endif // CALIBRATED_POSED_DEPTH_CAMERA_H 29 | -------------------------------------------------------------------------------- /src/camera_math.cuh: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef CAMERA_MATH_CUH 15 | #define CAMERA_MATH_CUH 16 | 17 | #include 18 | #include 19 | 20 | #include "libcgt/cuda/float4x4.h" 21 | 22 | // Convert a camera-space point into a pixel coordinate. 23 | // The camera-space point is right handed with, x-right, y-up, z out of screen. 24 | // The output pixel has x-right, y-up, and z equal to the -camera_point.z 25 | // (a depth value that is positive into the screen). 26 | __inline__ __device__ __host__ 27 | float3 PixelFromCamera(float3 camera_point, float4 flpp) { 28 | float depth = -camera_point.z; 29 | return{ 30 | flpp.x * camera_point.x / depth + flpp.z, 31 | flpp.y * camera_point.y / depth + flpp.w, 32 | depth 33 | }; 34 | } 35 | 36 | // Convert a world-space point into a pixel coordinate. 37 | // The output pixel has x-right, y-up, and z equal to the -camera_point.z 38 | // (a depth value that is positive into the screen). 39 | __inline__ __device__ __host__ 40 | float3 PixelFromWorld(float3 world_point, float4x4 camera_from_world, 41 | float4 flpp) { 42 | float3 camera_point = transformPoint(camera_from_world, world_point); 43 | return PixelFromCamera(camera_point, flpp); 44 | } 45 | 46 | // Convert a pixel coordinate to an camera-space point given depth. 47 | // The camera-space point is right handed with, x-right, y-up, z out of screen. 48 | // xy takes on integer values at pixel corners and half-integer values at pixel 49 | // centers. depth is a camera-space orthogonal depth and positive into the 50 | // screen. 51 | __inline__ __device__ __host__ 52 | float3 CameraFromPixel(float2 xy, float depth, float4 flpp) { 53 | return{ 54 | depth * (xy.x - flpp.z) / flpp.x, 55 | depth * (xy.y - flpp.w) / flpp.y, 56 | -depth 57 | }; 58 | } 59 | 60 | // Convert a pixel coordinate to an camera-space point given depth. 61 | // The camera-space point is right handed with, x-right, y-up, z out of screen. 62 | // xy is a pixel subscript and (0.5, 0.5) will be added to make it a pixel 63 | // center. depth is a camera-space orthogonal depth and positive into the 64 | // screen. 65 | __inline__ __device__ __host__ 66 | float3 CameraFromPixel(int2 xy, float depth, float4 flpp) { 67 | return CameraFromPixel( 68 | make_float2(xy.x + 0.5f, xy.y + 0.5f), 69 | depth, flpp 70 | ); 71 | } 72 | 73 | // Convert a pixel coordinate to a camera-space unit direction. 74 | // xy takes on integer values at pixel corners and half-integer values at pixel 75 | // centers. 76 | __inline__ __device__ __host__ 77 | float3 CameraDirectionFromPixel(float2 xy, float4 flpp) { 78 | float fx = flpp.x; 79 | float fy = flpp.y; 80 | float dx = xy.x - flpp.z; 81 | float dy = xy.y - flpp.w; 82 | float den = sqrt(dx * dx * fy * fy + dy * dy * fx * fx + fx * fx * fy * fy); 83 | return{ 84 | dx * fy / den, 85 | dy * fx / den, 86 | -fx * fy / den 87 | }; 88 | } 89 | 90 | // Convert a pixel coordinate to a camera-space unit direction. 91 | // xy is a pixel subscript and (0.5, 0.5) will be added to make it a pixel 92 | // center. 93 | __inline__ __device__ __host__ 94 | float3 CameraDirectionFromPixel(int2 xy, float4 flpp) { 95 | return CameraDirectionFromPixel( 96 | make_float2(xy.x + 0.5f, xy.y + 0.5f), 97 | flpp 98 | ); 99 | } 100 | 101 | // Convert a pixel coordinate to an world-space point given depth. 102 | // xy takes on integer values at pixel corners and half-integer values at pixel 103 | // centers. depth is a camera-space orthogonal depth. 104 | __inline__ __device__ __host__ 105 | float3 WorldFromPixel(float2 xy, float depth, 106 | float4 flpp, float4x4 world_from_camera) { 107 | return transformPoint(world_from_camera, CameraFromPixel(xy, depth, flpp)); 108 | } 109 | 110 | // Convert a pixel coordinate to an world-space point given depth. 111 | // xy is a pixel subscript and (0.5, 0.5) will be added to make it a pixel 112 | // center. depth is a camera-space orthogonal depth. 113 | __inline__ __device__ __host__ 114 | float3 WorldFromPixel(int2 xy, float depth, 115 | float4 flpp, float4x4 world_from_camera) { 116 | return transformPoint(world_from_camera, CameraFromPixel(xy, depth, flpp)); 117 | } 118 | 119 | // Convert a pixel to a world-space unit direction. 120 | // xy takes on integer values at pixel corners and half-integer values at pixel 121 | // centers. 122 | __inline__ __device__ __host__ 123 | float3 WorldDirectionFromPixel(float2 xy, float4 flpp, 124 | float4x4 world_from_camera) { 125 | float3 d = CameraDirectionFromPixel(xy, flpp); 126 | return transformVector(world_from_camera, d); 127 | } 128 | 129 | // Convert a pixel to a world-space unit direction. 130 | // xy is a pixel subscript and (0.5, 0.5) will be added to make it a pixel 131 | // center. 132 | __inline__ __device__ __host__ 133 | float3 WorldDirectionFromPixel(int2 xy, float4 flpp, 134 | float4x4 world_from_camera) { 135 | float3 d = CameraDirectionFromPixel(xy, flpp); 136 | return transformVector(world_from_camera, d); 137 | } 138 | 139 | #endif // CAMERA_MATH_CUH 140 | -------------------------------------------------------------------------------- /src/control_widget.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "control_widget.h" 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | ControlWidget::ControlWidget(QWidget* parent) : 21 | QWidget(parent) { 22 | QGridLayout* layout = new QGridLayout; 23 | 24 | QPushButton* pause_button = new QPushButton("Pause / Resume"); 25 | QObject::connect(pause_button, &QPushButton::clicked, 26 | this, &ControlWidget::pauseClicked); 27 | layout->addWidget(pause_button); 28 | 29 | QPushButton* step_frame_button = new QPushButton("Step One Frame"); 30 | QObject::connect(step_frame_button, &QPushButton::clicked, 31 | this, &ControlWidget::stepClicked); 32 | layout->addWidget(step_frame_button); 33 | 34 | QPushButton* reset_button = new QPushButton("Reset"); 35 | QObject::connect( reset_button, &QPushButton::clicked, 36 | this, &ControlWidget::resetClicked); 37 | layout->addWidget(reset_button); 38 | 39 | QPushButton* save_mesh_button = new QPushButton("Save Mesh"); 40 | QObject::connect(save_mesh_button, &QPushButton::clicked, 41 | this, &ControlWidget::OnSaveMeshClicked); 42 | layout->addWidget(save_mesh_button); 43 | 44 | QPushButton* save_pose_button = new QPushButton("Save Pose Stream"); 45 | QObject::connect(save_pose_button, &QPushButton::clicked, 46 | this, &ControlWidget::OnSavePoseClicked); 47 | layout->addWidget(save_pose_button); 48 | 49 | setLayout(layout); 50 | } 51 | 52 | void ControlWidget::OnSaveMeshClicked() { 53 | QString filename = QFileDialog::getSaveFileName(this, 54 | "Save Mesh", 55 | QString(), 56 | "Alias|Wavefront Meshes (*.obj)" 57 | ); 58 | if (filename != "") { 59 | emit saveMeshClicked(filename); 60 | } 61 | } 62 | 63 | void ControlWidget::OnSavePoseClicked() { 64 | QString filename = QFileDialog::getSaveFileName(this, 65 | "Save Pose Stream", 66 | QString(), 67 | "Pose Streams (*.pose)" 68 | ); 69 | 70 | if (filename != "") { 71 | emit savePoseClicked(filename); 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /src/control_widget.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef CONTROL_WIDGET_H 15 | #define CONTROL_WIDGET_H 16 | 17 | #include 18 | 19 | class ControlWidget : public QWidget { 20 | 21 | Q_OBJECT 22 | 23 | public: 24 | 25 | ControlWidget(QWidget* parent = 0); 26 | 27 | signals: 28 | 29 | void pauseClicked(); 30 | void stepClicked(); 31 | void resetClicked(); 32 | void saveMeshClicked(QString filename); 33 | void savePoseClicked(QString filename); 34 | 35 | private slots: 36 | 37 | void OnSaveMeshClicked(); 38 | void OnSavePoseClicked(); 39 | 40 | }; 41 | 42 | #endif // CONTROL_WIDGET_H 43 | -------------------------------------------------------------------------------- /src/depth_processor.cu: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "depth_processor.h" 15 | 16 | #include "libcgt/cuda/Event.h" 17 | #include "libcgt/cuda/MathUtils.h" 18 | #include "libcgt/cuda/Rect2i.h" 19 | #include "libcgt/cuda/ThreadMath.cuh" 20 | #include "libcgt/cuda/VecmathConversions.h" 21 | 22 | #include "camera_math.cuh" 23 | 24 | using libcgt::cuda::Event; 25 | using libcgt::cuda::threadmath::threadSubscript2DGlobal; 26 | using libcgt::cuda::contains; 27 | using libcgt::cuda::inset; 28 | using libcgt::cuda::math::numBins2D; 29 | 30 | __global__ 31 | void SmoothDepthMapKernel(KernelArray2D input, 32 | float2 depth_min_max, 33 | int kernel_radius, 34 | float delta_z_squared_threshold, 35 | KernelArray2D smoothed) { 36 | int2 xy = threadSubscript2DGlobal(); 37 | libcgt::cuda::Rect2i valid_rect = inset(libcgt::cuda::Rect2i(input.size()), 38 | { kernel_radius, kernel_radius }); 39 | float z = input[xy]; 40 | float smoothed_z = 0.0f; 41 | if (contains(valid_rect, xy) && 42 | z >= depth_min_max.x && z <= depth_min_max.y) { 43 | 44 | float sum = 0.0f; 45 | float sum_weights = 0.0f; 46 | 47 | for (int dy = -kernel_radius; dy <= kernel_radius; ++dy) { 48 | for (int dx = -kernel_radius; dx <= kernel_radius; ++dx) { 49 | float z2 = input[xy + int2{dx, dy}]; 50 | float delta_z = z2 - z; 51 | float delta_z_squared = delta_z * delta_z; 52 | if (z2 != 0 && delta_z_squared < delta_z_squared_threshold) { 53 | float dr2 = dx * dx + dy * dy; 54 | float dr = sqrt(dr2); 55 | // TODO(jiawen): Hacky bilateral filter without exp(). 56 | float spatial_weight = 1.0f / (1.0f + dr); 57 | float range_weight = delta_z_squared_threshold - delta_z_squared; 58 | float weight = spatial_weight * range_weight; 59 | sum += weight * z2; 60 | sum_weights += weight; 61 | } 62 | } 63 | } 64 | 65 | if (sum_weights > 0.0f) { 66 | smoothed_z = sum / sum_weights; 67 | } 68 | } 69 | smoothed[xy] = smoothed_z; 70 | } 71 | 72 | __global__ 73 | void UndistortKernel(cudaTextureObject_t raw_depth, 74 | cudaTextureObject_t undistort_map, 75 | KernelArray2D undistorted) { 76 | int2 xy = threadSubscript2DGlobal(); 77 | if (contains(libcgt::cuda::Rect2i(undistorted.size()), xy)) { 78 | // TODO: make a function to go from xy to [0,1] 79 | 80 | float u = xy.x + 0.5f; 81 | float v = xy.y + 0.5f; 82 | 83 | // Fetch from undistort_map[uv] to get xy2. 84 | float2 xy2 = tex2D(undistort_map, u, v); 85 | 86 | undistorted[xy] = tex2D(raw_depth, xy2.x, xy2.y); 87 | } 88 | } 89 | 90 | __global__ 91 | void EstimateNormalsKernel(KernelArray2D depth_map, 92 | float4 flpp, float2 depth_min_max, 93 | KernelArray2D normals) { 94 | int2 xy = threadSubscript2DGlobal(); 95 | float4 normal = {}; 96 | 97 | if (xy.x < depth_map.width() - 1 && xy.y < depth_map.height() - 1) { 98 | float depth0 = depth_map[xy]; 99 | int2 xy1{ xy.x + 1, xy.y }; 100 | int2 xy2{ xy.x, xy.y + 1 }; 101 | float depth1 = depth_map[xy1]; 102 | float depth2 = depth_map[xy2]; 103 | 104 | if (depth0 >= depth_min_max.x && depth0 <= depth_min_max.y && 105 | depth1 >= depth_min_max.x && depth1 <= depth_min_max.y && 106 | depth2 >= depth_min_max.x && depth2 <= depth_min_max.y) { 107 | 108 | // TODO: can optimize this by not using CameraFromPixel and directly 109 | // scaling x and y by z. 110 | float3 p0 = CameraFromPixel(xy, depth0, flpp); 111 | float3 p1 = CameraFromPixel(xy1, depth1, flpp); 112 | float3 p2 = CameraFromPixel(xy2, depth2, flpp); 113 | 114 | float3 dx = p1 - p0; 115 | float3 dy = p2 - p0; 116 | float3 n = cross(dx, dy); 117 | float lenSquared = lengthSquared(n); 118 | if (lenSquared > 0.0f) { 119 | normal = make_float4(n / sqrt(lenSquared), 1.0f); 120 | } 121 | } 122 | } 123 | 124 | normals[xy] = normal; 125 | } 126 | 127 | DepthProcessor::DepthProcessor(const Intrinsics& depth_intrinsics, 128 | const Range1f& depth_range) : 129 | depth_intrinsics_flpp_{ depth_intrinsics.focalLength, 130 | depth_intrinsics.principalPoint }, 131 | depth_range_(depth_range) { 132 | 133 | } 134 | 135 | void DepthProcessor::Undistort(DeviceArray2D& raw_depth, 136 | DeviceArray2D& undistort_map, 137 | DeviceArray2D& undistorted_depth) { 138 | // Bind raw_depth and undistort_map to texture objects. 139 | 140 | cudaResourceDesc raw_depth_res_desc = raw_depth.resourceDesc(); 141 | cudaResourceDesc undistort_map_res_desc = undistort_map.resourceDesc(); 142 | 143 | cudaTextureDesc point_normalized_tex_desc = {}; 144 | point_normalized_tex_desc.addressMode[0] = cudaAddressModeClamp; 145 | point_normalized_tex_desc.addressMode[1] = cudaAddressModeClamp; 146 | point_normalized_tex_desc.filterMode = cudaFilterModePoint; 147 | point_normalized_tex_desc.readMode = cudaReadModeElementType; 148 | point_normalized_tex_desc.normalizedCoords = true; 149 | 150 | cudaTextureDesc point_unnormalized_tex_desc = {}; 151 | point_unnormalized_tex_desc.addressMode[0] = cudaAddressModeClamp; 152 | point_unnormalized_tex_desc.addressMode[1] = cudaAddressModeClamp; 153 | point_unnormalized_tex_desc.filterMode = cudaFilterModePoint; 154 | point_unnormalized_tex_desc.readMode = cudaReadModeElementType; 155 | point_unnormalized_tex_desc.normalizedCoords = false; 156 | 157 | cudaError err; 158 | 159 | cudaTextureObject_t raw_depth_tex_obj; 160 | err = cudaCreateTextureObject(&raw_depth_tex_obj, &raw_depth_res_desc, 161 | &point_normalized_tex_desc, nullptr); 162 | 163 | cudaTextureObject_t undistort_map_tex_obj; 164 | err = cudaCreateTextureObject(&undistort_map_tex_obj, &undistort_map_res_desc, 165 | &point_unnormalized_tex_desc, nullptr); 166 | 167 | dim3 block(16, 16); 168 | dim3 grid = numBins2D(make_int2(raw_depth.size()), block); 169 | 170 | Event e; 171 | e.recordStart(); 172 | UndistortKernel<<>>( 173 | raw_depth_tex_obj, 174 | undistort_map_tex_obj, 175 | undistorted_depth.writeView()); 176 | float dtMS = e.recordStopSyncAndGetMillisecondsElapsed(); 177 | printf("DepthProcessor::Undistort took %f ms\n", dtMS); 178 | 179 | // TODO: don't destroy the texture every time. 180 | cudaDestroyTextureObject(undistort_map_tex_obj); 181 | cudaDestroyTextureObject(raw_depth_tex_obj); 182 | } 183 | 184 | void DepthProcessor::Smooth(DeviceArray2D& raw_depth, 185 | DeviceArray2D& smoothed_depth) { 186 | 187 | dim3 block(16, 16); 188 | dim3 grid = numBins2D(make_int2(raw_depth.size()), block); 189 | 190 | Event e; 191 | e.recordStart(); 192 | SmoothDepthMapKernel<<>>( 193 | raw_depth.readView(), 194 | make_float2(depth_range_.leftRight()), 195 | kernel_radius_, 196 | delta_z_squared_threshold_, 197 | smoothed_depth.writeView()); 198 | float dtMS = e.recordStopSyncAndGetMillisecondsElapsed(); 199 | printf("DepthProcessor::Smooth took %f ms\n", dtMS); 200 | } 201 | 202 | void DepthProcessor::EstimateNormals(DeviceArray2D& smoothed_depth, 203 | DeviceArray2D& normals) { 204 | dim3 block(16, 16); 205 | dim3 grid = numBins2D(make_int2(smoothed_depth.size()), block); 206 | 207 | Event e; 208 | e.recordStart(); 209 | EstimateNormalsKernel<<>>( 210 | smoothed_depth.readView(), 211 | make_float4(depth_intrinsics_flpp_), 212 | make_float2(depth_range_.leftRight()), 213 | normals.writeView()); 214 | float dtMS = e.recordStopSyncAndGetMillisecondsElapsed(); 215 | printf("DepthProcessor::EstimateNormals took %f ms\n", dtMS); 216 | } 217 | -------------------------------------------------------------------------------- /src/depth_processor.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef DEPTH_PROCESSOR_H 15 | #define DEPTH_PROCESSOR_H 16 | 17 | #include "libcgt/core/cameras/Camera.h" 18 | #include "libcgt/core/cameras/Intrinsics.h" 19 | #include "libcgt/core/vecmath/Range1f.h" 20 | #include "libcgt/core/vecmath/Vector4f.h" 21 | #include "libcgt/cuda/DeviceArray2D.h" 22 | 23 | class DepthProcessor { 24 | public: 25 | 26 | using Intrinsics = libcgt::core::cameras::Intrinsics; 27 | 28 | DepthProcessor(const Intrinsics& depth_intrinsics, 29 | const Range1f& depth_range); 30 | 31 | // TODO: document which direction is up. 32 | // Correct lens distortion in raw_depth using undistort_map. 33 | // TODO: switch interface to use textures as inputs. 34 | // TODO: switch interface to use surfaces as outputs. 35 | void Undistort(DeviceArray2D& raw_depth, 36 | DeviceArray2D& undistort_map, 37 | DeviceArray2D& undistorted_depth); 38 | 39 | // Smooth raw_depth with a bilateral filter. 40 | // TODO: replace hack with an actual bilateral filter. 41 | void Smooth(DeviceArray2D& raw_depth, 42 | DeviceArray2D& smoothed_depth); 43 | 44 | void EstimateNormals(DeviceArray2D& smoothed_depth, 45 | DeviceArray2D& normals); 46 | 47 | const Vector4f depth_intrinsics_flpp_; 48 | const Range1f depth_range_; 49 | const int kernel_radius_ = 2; 50 | const float delta_z_squared_threshold_ = 0.04f; // 40 mm for Kinect. 51 | 52 | }; 53 | 54 | #endif // DEPTH_PROCESSOR_H 55 | -------------------------------------------------------------------------------- /src/detector_params.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | nmarkers: 1024 3 | adaptiveThreshWinSize: 21 4 | adaptiveThreshWinSizeMin: 3 5 | adaptiveThreshWinSizeMax: 64 6 | adaptiveThreshWinSizeStep: 1 7 | adaptiveThreshConstant: 7 8 | minMarkerPerimeterRate: 0.03 9 | maxMarkerPerimeterRate: 4.0 10 | polygonalApproxAccuracyRate: 0.05 11 | minCornerDistance: 10.0 12 | minDistanceToBorder: 3 13 | minMarkerDistance: 10.0 14 | cornerRefinementWinSize: 5 15 | cornerRefinementMaxIterations: 30 16 | cornerRefinementMinAccuracy: 0.1 17 | markerBorderBits: 1 18 | perspectiveRemovePixelPerCell: 8 19 | perspectiveRemoveIgnoredMarginPerCell: 0.13 20 | maxErroneousBitsInBorderRate: 0.04 21 | -------------------------------------------------------------------------------- /src/fuse.cu: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "fuse.h" 15 | 16 | #include "libcgt/cuda/float4x4.h" 17 | #include "libcgt/cuda/MathUtils.h" 18 | #include "libcgt/cuda/ThreadMath.cuh" 19 | 20 | #include "camera_math.cuh" 21 | 22 | using libcgt::cuda::threadmath::threadSubscript2DGlobal; 23 | using libcgt::cuda::contains; 24 | using libcgt::cuda::math::roundToInt; 25 | 26 | __global__ 27 | void FuseKernel( 28 | float4x4 world_from_grid, 29 | float max_tsdf_value, 30 | float4 flpp, 31 | float2 depth_min_max, 32 | float4x4 camera_from_world, 33 | KernelArray2D depth_map, 34 | KernelArray3D regular_grid) { 35 | 36 | int2 ij = threadSubscript2DGlobal(); 37 | 38 | // Sweep over the entire volume. 39 | for (int k = 0; k < regular_grid.depth(); ++k) { 40 | // Find the voxel center. 41 | // TODO(jiawen): write a helper function that takes in a subscript 42 | float4 voxel_center_world = make_float4( 43 | transformPoint( 44 | world_from_grid, float3{ij.x + 0.5f, ij.y + 0.5f, k + 0.5f}), 45 | 1.0f); 46 | 47 | // Project it into camera coordinates. 48 | // camera_from_world uses OpenGL conventions, 49 | // so depth is a negative number if it's in front of the camera. 50 | float4 voxel_center_camera = 51 | camera_from_world * voxel_center_world; 52 | float2 uv = make_float2(PixelFromCamera(make_float3(voxel_center_camera), 53 | flpp)); 54 | int2 uv_int = roundToInt(uv - float2{ 0.5f, 0.5f }); 55 | 56 | if (voxel_center_camera.z > 0 || 57 | !contains(depth_map.size(), uv_int)) { 58 | continue; 59 | } 60 | 61 | float image_depth = depth_map[uv_int]; 62 | if (image_depth < depth_min_max.x || image_depth > depth_min_max.y) { 63 | continue; 64 | } 65 | 66 | // Compute dz, the signed distance between the voxel center and the 67 | // surface observation. 68 | // 69 | // Note that we flip the sign on z to get "depth", where positive numbers 70 | // are in front of the camera. 71 | // 72 | // The sign convention of the distance field is so that voxels in front of 73 | // the surface is positive (and voxels behind are negative). 74 | float voxel_center_depth = -voxel_center_camera.z; 75 | float dz = image_depth - voxel_center_depth; 76 | 77 | // Now integrate data in carefully: 78 | // Consider 3 cases: 79 | // dz < -max_tsdf_value: the voxel is behind the observation and out of the 80 | // truncation region. Therefore, do nothing. 81 | // dz \in [-max_tsdf_value, 0]: the voxel is behind the observation and 82 | // within the truncation region. Integrate. 83 | // dz > 0: the voxel is in front of the observation. Integrate... but if 84 | // the voxel is really far in front, we don't want to put in a large 85 | // value. Instead, clamp it to max_tsdf_value. 86 | if (dz >= -max_tsdf_value) { 87 | // Ignore the voxel when it is far behind. 88 | // Clamp to the TSDF range. 89 | dz = min(dz, max_tsdf_value); 90 | const float weight = 1.0f; 91 | 92 | regular_grid[{ij.x, ij.y, k}].Update(dz, weight, max_tsdf_value); 93 | } 94 | } 95 | } 96 | 97 | namespace { 98 | constexpr int kNumDepthMaps = 3; 99 | } 100 | 101 | __global__ 102 | void FuseMultipleKernel( 103 | float4x4 world_from_grid, 104 | float max_tsdf_value, 105 | CalibratedPosedDepthCamera depth_camera0, 106 | CalibratedPosedDepthCamera depth_camera1, 107 | CalibratedPosedDepthCamera depth_camera2, 108 | KernelArray2D depth_map0, 109 | KernelArray2D depth_map1, 110 | KernelArray2D depth_map2, 111 | KernelArray3D regular_grid) { 112 | 113 | CalibratedPosedDepthCamera depth_camera[] = 114 | { 115 | depth_camera0, 116 | depth_camera1, 117 | depth_camera2 118 | }; 119 | KernelArray2D depth_maps[] = 120 | { 121 | depth_map0, 122 | depth_map1, 123 | depth_map2 124 | }; 125 | 126 | int2 ij = threadSubscript2DGlobal(); 127 | 128 | // Sweep over the entire volume. 129 | for (int k = 0; k < regular_grid.depth(); ++k) { 130 | // Find the voxel center. 131 | // TODO(jiawen): write a helper function that takes in a subscript 132 | float4 voxel_center_world = make_float4( 133 | transformPoint( 134 | world_from_grid, float3{ij.x + 0.5f, ij.y + 0.5f, k + 0.5f}), 135 | 1.0f); 136 | 137 | for (int c = 0; c < kNumDepthMaps; ++c) { 138 | // Project it into camera coordinates. 139 | // camera_from_world uses OpenGL conventions, 140 | // so depth is a negative number if it's in front of the camera. 141 | float4 voxel_center_camera = 142 | depth_camera[c].camera_from_world * voxel_center_world; 143 | float2 uv = make_float2( 144 | PixelFromCamera(make_float3(voxel_center_camera), depth_camera[c].flpp)); 145 | int2 uv_int = roundToInt(uv - float2{0.5f, 0.5f}); 146 | 147 | if (voxel_center_camera.z > 0 || 148 | !contains( depth_maps[c].size(), uv_int)) { 149 | continue; 150 | } 151 | 152 | float image_depth = depth_maps[c][uv_int]; 153 | if (image_depth < depth_camera[c].depth_min_max.x || 154 | image_depth > depth_camera[c].depth_min_max.y) { 155 | continue; 156 | } 157 | 158 | // Compute dz, the signed distance between the voxel center and the 159 | // surface observation. 160 | // 161 | // Note that we flip the sign on z to get "depth", where positive numbers 162 | // are in front of the camera. 163 | // 164 | // The sign convention of the distance field is so that voxels in front of 165 | // the surface is positive (and voxels behind are negative). 166 | float voxel_center_depth = -voxel_center_camera.z; 167 | float dz = image_depth - voxel_center_depth; 168 | 169 | // Now integrate data in carefully: 170 | // Consider 3 cases: 171 | // dz < -max_tsdf_value: the voxel is behind the observation and out of the 172 | // truncation region. Therefore, do nothing. 173 | // dz \in [-max_tsdf_value, 0]: the voxel is behind the observation and 174 | // within the truncation region. Integrate. 175 | // dz > 0: the voxel is in front of the observation. Integrate... but if 176 | // the voxel is really far in front, we don't want to put in a large 177 | // value. Instead, clamp it to max_tsdf_value. 178 | if (dz >= -max_tsdf_value) { 179 | // Ignore the voxel when it is far behind. 180 | // Clamp to the TSDF range. 181 | dz = min(dz, max_tsdf_value); 182 | const float weight = 1.0f; 183 | 184 | regular_grid[{ij.x, ij.y, k}].Update(dz, weight, max_tsdf_value); 185 | } 186 | } 187 | } 188 | } 189 | -------------------------------------------------------------------------------- /src/fuse.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef FUSE_H 15 | #define FUSE_H 16 | 17 | #include 18 | 19 | #include "libcgt/cuda/float4x4.h" 20 | #include "libcgt/cuda/KernelArray2D.h" 21 | #include "libcgt/cuda/KernelArray3D.h" 22 | 23 | #include "calibrated_posed_depth_camera.h" 24 | #include "regular_grid_tsdf.h" 25 | 26 | __global__ 27 | void FuseKernel( 28 | float4x4 world_from_grid, 29 | float max_tsdf_value, 30 | float4 flpp, 31 | float2 depth_min_max, 32 | float4x4 camera_from_world, 33 | KernelArray2D depth_map, 34 | KernelArray3D regular_grid); 35 | 36 | // TODO: replace CalibratedPosedDepthCamera with something in __constant__ 37 | // memory. 38 | __global__ 39 | void FuseMultipleKernel( 40 | float4x4 world_from_grid, 41 | float max_tsdf_value, 42 | CalibratedPosedDepthCamera depth_camera0, 43 | CalibratedPosedDepthCamera depth_camera1, 44 | CalibratedPosedDepthCamera depth_camera2, 45 | KernelArray2D depth_map0, 46 | KernelArray2D depth_map1, 47 | KernelArray2D depth_map2, 48 | KernelArray3D regular_grid); 49 | 50 | #endif // FUSE_H 51 | -------------------------------------------------------------------------------- /src/fuse_depth/fuse_depth_cli.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | 16 | #include 17 | #include "libcgt/camera_wrappers/StreamConfig.h" 18 | #include "libcgt/core/vecmath/EuclideanTransform.h" 19 | #include "libcgt/core/vecmath/SimilarityTransform.h" 20 | 21 | #include "../input_buffer.h" 22 | #include "../pose_utils.h" 23 | #include "../regular_grid_fusion_pipeline.h" 24 | #include "../rgbd_camera_parameters.h" 25 | #include "../rgbd_input.h" 26 | 27 | using libcgt::core::vecmath::EuclideanTransform; 28 | using libcgt::core::vecmath::SimilarityTransform; 29 | 30 | // Inputs. 31 | DEFINE_string(calibration_dir, "", 32 | "[Required] calibration directory for the RGBD camera."); 33 | DEFINE_string(input_rgbd, "", 34 | "[Required] input .rgbd file."); 35 | DEFINE_string(pose_estimator, "color_aruco_and_depth_icp", 36 | "[Required] Pose estimator. Valid options: \"color_aruco\", \"depth_icp\", " 37 | "\"color_aruco_and_depth_icp\", \"precomputed\" or " 38 | "\"precomputed_refine_with_depth_icp\"." 39 | "If \"precomputed\" or \"precomputed_refine_with_depth_icp\", " 40 | "--precomputed_pose is required."); 41 | DEFINE_string(precomputed_pose, "", 42 | "[Optional] precomputed pose file."); 43 | 44 | // Outputs. 45 | DEFINE_string(output_mesh, "", 46 | "[Optional] If not-empty, save fused mesh as a .obj file."); 47 | DEFINE_string(output_pose, "", 48 | "[Optional] If not-empty, save new pose estimates as a .pose file."); 49 | DEFINE_string(output_tsdf3d, "", 50 | "[Optional] If non-empty, save the TSDF volume as a .tsdf3d file."); 51 | 52 | // Options. 53 | DEFINE_bool(collect_perf, false, "Collect performance statistics."); 54 | DEFINE_bool(adaptive_raycast, true, "Use signed distance values themselves " 55 | "during raycasting rather than one voxel at a time. Much faster, slightly " 56 | "less accurate."); 57 | 58 | // TODO: specify these as flags. 59 | constexpr int kRegularGridResolution = 512; 60 | constexpr float kRegularGridSideLength = 2.0f; 61 | constexpr float kRegularGridVoxelSize = 62 | kRegularGridSideLength / kRegularGridResolution; 63 | 64 | SimilarityTransform GetInitialWorldFromGrid() { 65 | // TODO: consider initializing the camera to be at the origin. 66 | if (FLAGS_pose_estimator == "depth_icp") { 67 | // Put the camera at the center of the front face of the cube. 68 | return SimilarityTransform(kRegularGridVoxelSize) * 69 | SimilarityTransform(Vector3f(-0.5f * kRegularGridResolution, 70 | -0.5f * kRegularGridResolution, 71 | -kRegularGridResolution)); 72 | } else { 73 | // Put the origin at the bottom in y, centered in x and z. 74 | return SimilarityTransform(Vector3f(-0.5f * kRegularGridSideLength, 75 | 0.0f, 76 | -0.5f * kRegularGridSideLength)) * 77 | SimilarityTransform(kRegularGridVoxelSize); 78 | } 79 | } 80 | 81 | bool GetPoseEstimatorOptions(const RGBDCameraParameters& camera_params, 82 | PoseEstimatorOptions* options) { 83 | if (FLAGS_pose_estimator == "color_aruco" || 84 | FLAGS_pose_estimator == "color_aruco_and_depth_icp") { 85 | if (FLAGS_pose_estimator == "color_aruco") { 86 | options->method = PoseEstimationMethod::COLOR_ARUCO; 87 | } else { 88 | options->method = PoseEstimationMethod::COLOR_ARUCO_AND_DEPTH_ICP; 89 | } 90 | return true; 91 | } else if (FLAGS_pose_estimator == "depth_icp") { 92 | // y up 93 | const EuclideanTransform kInitialDepthCameraFromWorld = 94 | EuclideanTransform::fromMatrix( 95 | Matrix4f::lookAt( 96 | { 0, 0, camera_params.depth.depth_range.minimum() }, 97 | Vector3f{ 0 }, 98 | Vector3f{ 0, 1, 0 }.normalized() 99 | ) 100 | ); 101 | 102 | options->method = PoseEstimationMethod::DEPTH_ICP; 103 | options->initial_pose.depth_camera_from_world = 104 | kInitialDepthCameraFromWorld; 105 | options->initial_pose.color_camera_from_world = 106 | camera_params.ConvertToColorCameraFromWorld( 107 | kInitialDepthCameraFromWorld); 108 | 109 | return true; 110 | } else if (FLAGS_pose_estimator == "precomputed" || 111 | FLAGS_pose_estimator == "precomputed_refine_with_depth_icp") { 112 | if (FLAGS_pose_estimator == "precomputed") { 113 | options->method = PoseEstimationMethod::PRECOMPUTED; 114 | } else { 115 | options->method = 116 | PoseEstimationMethod::PRECOMPUTED_REFINE_WITH_DEPTH_ICP; 117 | } 118 | options->precomputed_path = LoadPoseHistory(FLAGS_precomputed_pose, 119 | camera_params.depth_from_color); 120 | if (options->precomputed_path.size() == 0) { 121 | fprintf(stderr, "Error: failed to load precomputed poses from %s\n", 122 | FLAGS_precomputed_pose.c_str()); 123 | return false; 124 | } 125 | 126 | return true; 127 | } else { 128 | fprintf(stderr, "Invalid pose estimator: %s.\n", 129 | FLAGS_pose_estimator.c_str()); 130 | return false; 131 | } 132 | } 133 | 134 | int main(int argc, char* argv[]) { 135 | gflags::ParseCommandLineFlags(&argc, &argv, true); 136 | 137 | // If no outputs, return immediately. 138 | if (FLAGS_output_mesh == "" && 139 | FLAGS_output_pose == "" && 140 | FLAGS_output_tsdf3d == "") { 141 | fprintf(stderr, "No outputs specified, returning immediately.\n"); 142 | return 1; 143 | } 144 | 145 | bool ok; 146 | 147 | RGBDCameraParameters camera_params; 148 | ok = LoadRGBDCameraParameters(FLAGS_calibration_dir, &camera_params); 149 | if (!ok) { 150 | fprintf(stderr, "Error loading RGBD camera parameters from %s.\n", 151 | FLAGS_calibration_dir.c_str()); 152 | return 2; 153 | } 154 | 155 | // TODO: validate rgbd input size with camera calibration size. 156 | // It may not have a color stream. 157 | RgbdInput rgbd_input(RgbdInput::InputType::FILE, FLAGS_input_rgbd.c_str()); 158 | 159 | PoseEstimatorOptions pose_options; 160 | ok = GetPoseEstimatorOptions(camera_params, &pose_options); 161 | if (!ok) { 162 | fprintf(stderr, "Failed to parse pose estimator options."); 163 | return 2; 164 | } 165 | fprintf(stderr, "Using pose estimator: %s\n", FLAGS_pose_estimator.c_str()); 166 | 167 | RegularGridFusionPipeline pipeline(camera_params, 168 | Vector3i(kRegularGridResolution), 169 | GetInitialWorldFromGrid(), 170 | pose_options); 171 | 172 | bool color_updated; 173 | bool depth_updated; 174 | rgbd_input.read(&(pipeline.GetInputBuffer()), 175 | &color_updated, 176 | &depth_updated); 177 | while (color_updated || depth_updated) { 178 | if (color_updated) { 179 | pipeline.NotifyColorUpdated(); 180 | } else if (depth_updated) { 181 | pipeline.NotifyDepthUpdated(); 182 | } 183 | rgbd_input.read(&(pipeline.GetInputBuffer()), 184 | &color_updated, 185 | &depth_updated); 186 | } 187 | 188 | // Fusion finished, save outputs. 189 | if (FLAGS_output_mesh != "") { 190 | TriangleMesh mesh = pipeline.Triangulate(); 191 | fprintf(stderr, "Saving mesh to %s...", FLAGS_output_mesh.c_str()); 192 | ok = mesh.saveOBJ(FLAGS_output_mesh); 193 | if (ok) { 194 | fprintf(stderr, "done.\n"); 195 | } else { 196 | fprintf(stderr, "FAILED.\n"); 197 | } 198 | } 199 | 200 | if (FLAGS_output_pose != "") { 201 | printf("Saving poses to %s...", FLAGS_output_pose.c_str()); 202 | ok = SavePoseHistory(pipeline.PoseHistory(), FLAGS_output_pose); 203 | if (ok) { 204 | fprintf(stderr, "done.\n"); 205 | } else { 206 | fprintf(stderr, "FAILED.\n"); 207 | } 208 | } 209 | 210 | if (FLAGS_output_tsdf3d != "") { 211 | printf("Saving TSDF volume to %s...", FLAGS_output_tsdf3d.c_str()); 212 | ok = pipeline.SaveTSDF3D(FLAGS_output_tsdf3d); 213 | if (ok) { 214 | fprintf(stderr, "done.\n"); 215 | } else { 216 | fprintf(stderr, "FAILED.\n"); 217 | } 218 | } 219 | } 220 | -------------------------------------------------------------------------------- /src/icp_least_squares_data.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "icp_least_squares_data.h" 15 | 16 | #include "third_party/Eigen/Eigen/Dense" 17 | 18 | void Solve(const ICPLeastSquaresData& system, float x[6]) { 19 | Eigen::Matrix a; 20 | Eigen::Matrix b; 21 | int k = 0; 22 | for (int i = 0; i < 6; ++i) { 23 | for (int j = i; j < 6; ++j) { 24 | a(i, j) = system.a[k]; 25 | a(j, i) = system.a[k]; 26 | ++k; 27 | } 28 | b(i, 0) = system.b[i]; 29 | } 30 | 31 | Eigen::Map> x_map(x); 32 | x_map = a.colPivHouseholderQr().solve(b); 33 | } 34 | -------------------------------------------------------------------------------- /src/icp_least_squares_data.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef ICP_LEAST_SQUARES_DATA_H 15 | #define ICP_LEAST_SQUARES_DATA_H 16 | 17 | struct ICPLeastSquaresData { 18 | // Packed storage for the symmetric covariance matrix. 19 | float a[21]; 20 | 21 | // Column major storage for the right hand side vector. 22 | float b[6]; 23 | 24 | // Aka the L2 energy. 25 | float squared_residual; 26 | 27 | // The number of samples accumulated to form a and b. 28 | int num_samples; 29 | }; 30 | 31 | void Solve(const ICPLeastSquaresData& system, float x[6]); 32 | 33 | #endif // ICP_LEAST_SQUARES_DATA_H 34 | -------------------------------------------------------------------------------- /src/input_buffer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "input_buffer.h" 15 | 16 | InputBuffer::InputBuffer(const Vector2i& color_resolution, 17 | const Vector2i& depth_resolution) : 18 | color_bgr_ydown(color_resolution), 19 | color_rgb(color_resolution), 20 | depth_meters(depth_resolution) { 21 | 22 | } 23 | -------------------------------------------------------------------------------- /src/input_buffer.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef INPUT_BUFFER_H 15 | #define INPUT_BUFFER_H 16 | 17 | #include "libcgt/core/common/Array2D.h" 18 | #include "libcgt/core/common/BasicTypes.h" 19 | #include "libcgt/core/vecmath/EuclideanTransform.h" 20 | #include "libcgt/core/vecmath/Vector2i.h" 21 | 22 | struct InputBuffer { 23 | 24 | InputBuffer(const Vector2i& color_resolution, 25 | const Vector2i& depth_resolution); 26 | 27 | // Incoming metadata. 28 | int color_frame_index = 0; 29 | int64_t color_timestamp_ns = 0LL; 30 | int depth_frame_index = 0; 31 | int64_t depth_timestamp_ns = 0LL; 32 | 33 | // This buffer is y-down for OpenCV only. 34 | Array2D color_bgr_ydown; 35 | 36 | // These buffers are y-up for processing and GL. 37 | Array2D color_rgb; 38 | Array2D depth_meters; // depth in meters 39 | }; 40 | 41 | #endif // INPUT_BUFFER_H 42 | -------------------------------------------------------------------------------- /src/interpolate_depth_pose/interpolate_depth_pose_cli.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include "libcgt/camera_wrappers/PoseStream.h" 5 | #include "libcgt/camera_wrappers/RGBDStream.h" 6 | #include "libcgt/core/math/MathUtils.h" 7 | #include "libcgt/core/vecmath/EuclideanTransform.h" 8 | 9 | #include "../rgbd_camera_parameters.h" 10 | 11 | using libcgt::camera_wrappers::PoseInputStream; 12 | using libcgt::camera_wrappers::PoseOutputStream; 13 | using libcgt::camera_wrappers::PoseStreamFormat; 14 | using libcgt::camera_wrappers::PoseStreamMetadata; 15 | using libcgt::camera_wrappers::PoseStreamTransformDirection; 16 | using libcgt::camera_wrappers::PoseStreamUnits; 17 | using libcgt::camera_wrappers::RGBDInputStream; 18 | using libcgt::core::math::fraction; 19 | using libcgt::core::vecmath::EuclideanTransform; 20 | using libcgt::core::vecmath::lerp; 21 | 22 | DEFINE_string(calibration_dir, "", 23 | "calibration directory for the RGBD camera."); 24 | 25 | DEFINE_string(reference_pose, "", "Reference camera path file (.pose)."); 26 | 27 | DEFINE_string(input_rgbd, "", "Input RGBD stream file (.rgbd) whose depth" 28 | " positions should be interpolated."); 29 | 30 | DEFINE_string(output_merged_pose, "", "Filename (.pose) for merged output" 31 | " stream."); 32 | 33 | struct PoseFrame { 34 | int32_t frame_index; 35 | int64_t timestamp; 36 | EuclideanTransform e; 37 | }; 38 | 39 | bool less(const PoseFrame& x, const PoseFrame& y) { 40 | return x.timestamp < y.timestamp; 41 | } 42 | 43 | std::vector LoadPoses(const std::string& filename, 44 | PoseStreamMetadata& metadata) { 45 | PoseInputStream inputStream(filename.c_str()); 46 | metadata = inputStream.metadata(); 47 | std::vector poses; 48 | PoseFrame f; 49 | bool ok = inputStream.read(f.frame_index, f.timestamp, 50 | f.e.rotation, f.e.translation); 51 | while(ok) 52 | { 53 | poses.push_back(f); 54 | ok = inputStream.read(f.frame_index, f.timestamp, 55 | f.e.rotation, f.e.translation); 56 | } 57 | return poses; 58 | } 59 | 60 | std::vector> LoadDepthTimestamps(const std::string& rgbd_filename) { 61 | std::vector> output; 62 | RGBDInputStream stream(rgbd_filename.c_str()); 63 | 64 | // Find metadata stream id. 65 | int depth_stream_id = -1; 66 | for (size_t i = 0; i < stream.metadata().size(); ++i) { 67 | if (stream.metadata()[i].type == StreamType::DEPTH) { 68 | depth_stream_id = static_cast(i); 69 | } 70 | } 71 | 72 | if (depth_stream_id == -1) { 73 | return output; 74 | } 75 | 76 | uint32_t stream_id; 77 | int32_t frame_index; 78 | int64_t timestamp; 79 | while(stream.read(stream_id, frame_index, timestamp).notNull()) { 80 | if (stream_id == depth_stream_id) { 81 | output.push_back(std::make_pair(frame_index, timestamp)); 82 | } 83 | } 84 | 85 | return output; 86 | } 87 | 88 | float fraction(int64_t x, int64_t lo, int64_t size) { 89 | return static_cast(x - lo) / size; 90 | } 91 | 92 | int main(int argc, char* argv[]) { 93 | gflags::ParseCommandLineFlags(&argc, &argv, true); 94 | if (FLAGS_calibration_dir == "") { 95 | printf("calibration_dir is required.\n"); 96 | return 1; 97 | } 98 | if (FLAGS_reference_pose == "") { 99 | printf("reference_pose is required.\n"); 100 | } 101 | if (FLAGS_reference_pose == "") { 102 | printf("reference_pose is required.\n"); 103 | } 104 | if (FLAGS_input_rgbd == "") { 105 | printf("input_rgbd is required.\n"); 106 | return 1; 107 | } 108 | if (FLAGS_output_merged_pose == "") { 109 | printf("output_merged_pose is required.\n"); 110 | return 1; 111 | } 112 | 113 | RGBDCameraParameters params; 114 | bool ok = LoadRGBDCameraParameters(FLAGS_calibration_dir, ¶ms); 115 | if (!ok) { 116 | fprintf(stderr, "Error loading RGBD camera parameters from %s.\n", 117 | FLAGS_calibration_dir.c_str()); 118 | return 1; 119 | } 120 | 121 | PoseStreamMetadata input_metadata; 122 | auto sfm_aligned_poses = LoadPoses(FLAGS_reference_pose, input_metadata); 123 | int num_sfm_poses = static_cast(sfm_aligned_poses.size()); 124 | 125 | std::vector sfm_aligned_timestamps(sfm_aligned_poses.size()); 126 | for (size_t i = 0; i < sfm_aligned_poses.size(); ++i) { 127 | sfm_aligned_timestamps[i] = sfm_aligned_poses[i].timestamp; 128 | } 129 | 130 | auto depth_timestamps = LoadDepthTimestamps(FLAGS_input_rgbd); 131 | int num_depth_timestamps = static_cast(depth_timestamps.size()); 132 | 133 | std::vector depth_poses; 134 | for (const auto& ft : depth_timestamps) { 135 | PoseFrame depth_pose; 136 | depth_pose.frame_index = ft.first; 137 | depth_pose.timestamp = ft.second; 138 | 139 | // Find the pair of timestamps in sfm_aligned_poses bracketing 140 | // depth_timestamp. 141 | int lbIndex = static_cast( 142 | std::lower_bound(sfm_aligned_timestamps.begin(), 143 | sfm_aligned_timestamps.end(), 144 | depth_pose.timestamp) - 145 | sfm_aligned_timestamps.begin()); 146 | 147 | // Lower bound is past the edge, do nothing. 148 | if (lbIndex == num_sfm_poses) { 149 | continue; 150 | } 151 | 152 | int64_t lbt = sfm_aligned_timestamps[lbIndex]; 153 | 154 | // Exact match: do nothing - no need to lerp since it will already exist. 155 | if (lbt == depth_pose.timestamp) { 156 | continue; 157 | } 158 | 159 | // Lower bound is not an exact match and it's the first index. This means 160 | // there's no earlier frame from which to interpolate, do nothing. 161 | if (lbIndex == 0) { 162 | continue; 163 | } 164 | 165 | // Lerp. 166 | int64_t t0 = sfm_aligned_timestamps[lbIndex - 1]; 167 | int64_t t1 = lbt; 168 | float f = fraction(depth_pose.timestamp, t0, t1 - t0); 169 | PoseFrame p0 = sfm_aligned_poses[lbIndex - 1]; 170 | PoseFrame p1 = sfm_aligned_poses[lbIndex]; 171 | 172 | depth_pose.e = lerp(p0.e, p1.e, f); 173 | depth_poses.push_back(depth_pose); 174 | } 175 | 176 | std::vector merged_poses(num_sfm_poses + depth_poses.size()); 177 | std::merge(sfm_aligned_poses.begin(), sfm_aligned_poses.end(), 178 | depth_poses.begin(), depth_poses.end(), 179 | merged_poses.begin(), less); 180 | 181 | PoseStreamMetadata output_metadata = input_metadata; 182 | PoseOutputStream output_stream(output_metadata, FLAGS_output_merged_pose); 183 | for (PoseFrame p : merged_poses) { 184 | output_stream.write(p.frame_index, p.timestamp, 185 | p.e.rotation, p.e.translation); 186 | } 187 | } 188 | -------------------------------------------------------------------------------- /src/main_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "main_controller.h" 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "libcgt/core/common/ArrayUtils.h" 21 | #include "libcgt/core/io/PortableFloatMapIO.h" 22 | #include "libcgt/camera_wrappers/PoseStream.h" 23 | 24 | #include "control_widget.h" 25 | #include "main_widget.h" 26 | #include "pose_utils.h" 27 | #include "rgbd_input.h" 28 | 29 | DECLARE_string(mode); 30 | DECLARE_string(sm_input_type); 31 | 32 | namespace { 33 | constexpr int kTimestampFieldWidth = 20; 34 | } 35 | 36 | MainController::MainController( 37 | RgbdInput* input, RegularGridFusionPipeline* pipeline, 38 | ControlWidget* control_widget, MainWidget* main_widget) : 39 | input_(input), 40 | pipeline_(pipeline), 41 | main_widget_(main_widget) { 42 | read_input_timer_ = new QTimer(this); 43 | read_input_timer_->setInterval(0); 44 | QObject::connect(read_input_timer_, &QTimer::timeout, 45 | this, &MainController::OnReadInput); 46 | 47 | QObject::connect(control_widget, &ControlWidget::pauseClicked, 48 | this, &MainController::OnPauseClicked); 49 | QObject::connect(control_widget, &ControlWidget::stepClicked, 50 | this, &MainController::OnReadInput); 51 | QObject::connect(control_widget, &ControlWidget::resetClicked, 52 | this, &MainController::OnResetClicked); 53 | QObject::connect(control_widget, &ControlWidget::saveMeshClicked, 54 | this, &MainController::OnSaveMeshClicked); 55 | QObject::connect(control_widget, &ControlWidget::savePoseClicked, 56 | this, &MainController::OnSavePoseClicked); 57 | QObject::connect(pipeline, &RegularGridFusionPipeline::dataChanged, 58 | main_widget_->GetSingleMovingCameraGLState(), 59 | &SingleMovingCameraGLState::OnPipelineDataChanged); 60 | } 61 | 62 | void MainController::OnReadInput() { 63 | static int i = 0; 64 | if (FLAGS_mode == "single_moving") { 65 | bool color_updated; 66 | bool depth_updated; 67 | // TODO: read should report a status: 68 | // COLOR_UPDATED, DEPTH_UPDATED, TIMEOUT or END_OF_FILE. 69 | input_->read(&(pipeline_->GetInputBuffer()), 70 | &color_updated, &depth_updated); 71 | // TODO: read just color or depth, not both. 72 | if (color_updated || depth_updated) { 73 | if (color_updated) { 74 | pipeline_->NotifyColorUpdated(); 75 | } else if (depth_updated) { 76 | pipeline_->NotifyDepthUpdated(); 77 | } 78 | } else if (FLAGS_sm_input_type == "file") { 79 | read_input_timer_->stop(); 80 | } 81 | } else if (FLAGS_mode == "multi_static") { 82 | for (int i = 0; i < static_cast(inputs_.size()); ++i) { 83 | bool color_updated; 84 | bool depth_updated; 85 | inputs_[i].read(&(msc_pipeline_->GetInputBuffer(i)), 86 | &color_updated, &depth_updated); 87 | if (color_updated || depth_updated) { 88 | msc_pipeline_->NotifyInputUpdated(i, color_updated, depth_updated); 89 | } else if (FLAGS_sm_input_type == "file") { 90 | read_input_timer_->stop(); 91 | } 92 | } 93 | 94 | //if(depth_updated) { 95 | { 96 | // TODO: pipeline should emit that TSDF has changed. 97 | msc_pipeline_->Reset(); 98 | msc_pipeline_->FuseMultiple(); 99 | main_widget_->GetMultiStaticCameraGLState()->NotifyTSDFUpdated(); 100 | } 101 | 102 | //if(rgb_updated || depth_updated) { 103 | { 104 | main_widget_->update(); 105 | } 106 | } 107 | } 108 | 109 | void MainController::OnPauseClicked() { 110 | if (read_input_timer_->isActive()) { 111 | read_input_timer_->stop(); 112 | } else { 113 | read_input_timer_->start(); 114 | } 115 | } 116 | 117 | void MainController::OnResetClicked() { 118 | // TODO: reset rgbd and pose streams. 119 | if (pipeline_ != nullptr) { 120 | pipeline_->Reset(); 121 | } 122 | if (msc_pipeline_ != nullptr) { 123 | msc_pipeline_->Reset(); 124 | } 125 | } 126 | 127 | void MainController::OnSaveMeshClicked(QString filename) { 128 | if (FLAGS_mode == "single_moving") { 129 | TriangleMesh mesh = pipeline_->Triangulate(); 130 | bool succeeded = mesh.saveOBJ(filename.toStdString()); 131 | if (!succeeded) { 132 | QMessageBox::critical(main_widget_, "Save Mesh Status", 133 | "Failed to save to: " + filename); 134 | } 135 | } else if (FLAGS_mode == "multi_static") { 136 | // HACK: rot180 137 | Matrix4f rot180 = Matrix4f::rotateX(static_cast(M_PI)); 138 | TriangleMesh mesh = msc_pipeline_->Triangulate(rot180); 139 | bool succeeded = mesh.saveOBJ(filename.toStdString()); 140 | if (!succeeded) { 141 | QMessageBox::critical(main_widget_, "Save Mesh Status", 142 | "Failed to save to: " + filename); 143 | } 144 | } 145 | } 146 | 147 | void MainController::OnSavePoseClicked(QString filename) { 148 | if (filename != "") { 149 | printf("Saving pose stream to %s...", filename.toStdString().c_str()); 150 | bool succeeded = SavePoseHistory( 151 | pipeline_->PoseHistory(), filename.toStdString()); 152 | if (succeeded) { 153 | printf("succeeded.\n"); 154 | } else { 155 | printf("failed.\n"); 156 | QMessageBox::critical(main_widget_, "Save Pose Stream Status", 157 | "Failed to save to: " + filename); 158 | } 159 | } 160 | } 161 | -------------------------------------------------------------------------------- /src/main_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MAIN_CONTROLLER_H 15 | #define MAIN_CONTROLLER_H 16 | 17 | #include 18 | #include 19 | 20 | #include "regular_grid_fusion_pipeline.h" 21 | #include "multi_static_camera_pipeline.h" 22 | 23 | class ControlWidget; 24 | class MainWidget; 25 | class QTimer; 26 | class RgbdInput; 27 | 28 | class MainController : public QObject { 29 | 30 | Q_OBJECT 31 | 32 | public: 33 | 34 | MainController(RgbdInput* input, RegularGridFusionPipeline* pipeline, 35 | ControlWidget* control_widget, MainWidget* main_widget); 36 | 37 | // HACK 38 | std::vector inputs_; 39 | MultiStaticCameraPipeline* msc_pipeline_ = nullptr; 40 | 41 | public slots: 42 | 43 | void OnPauseClicked(); 44 | void OnResetClicked(); 45 | void OnSaveMeshClicked(QString filename); 46 | void OnSavePoseClicked(QString filename); 47 | 48 | private slots: 49 | 50 | void OnReadInput(); 51 | 52 | private: 53 | 54 | // Data. 55 | RgbdInput* input_ = nullptr; 56 | RegularGridFusionPipeline* pipeline_ = nullptr; 57 | 58 | // GUI. 59 | ControlWidget* control_widget_ = nullptr; 60 | MainWidget* main_widget_ = nullptr; 61 | 62 | QTimer* read_input_timer_ = nullptr; 63 | }; 64 | 65 | #endif // MAIN_CONTROLLER_H 66 | -------------------------------------------------------------------------------- /src/main_widget.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "main_widget.h" 15 | 16 | #include 17 | 18 | #include "libcgt/core/common/ArrayView.h" 19 | #include "libcgt/core/common/ArrayUtils.h" 20 | #include "libcgt/core/geometry/RectangleUtils.h" 21 | #include "libcgt/core/imageproc/ColorMap.h" 22 | #include "libcgt/GL/GLUtilities.h" 23 | 24 | #include "input_buffer.h" 25 | #include "regular_grid_fusion_pipeline.h" 26 | 27 | const float kFreeCameraFovYRadians = 0.87f; // 50 degrees. 28 | const float kFreeCameraZNear = 0.001f; 29 | const float kFreeCameraZFar = 100.0f; 30 | 31 | using libcgt::core::arrayutils::cast; 32 | using libcgt::core::cameras::GLFrustum; 33 | using libcgt::core::cameras::Intrinsics; 34 | using libcgt::core::imageproc::linearRemapToLuminance; 35 | using libcgt::core::vecmath::EuclideanTransform; 36 | using libcgt::core::vecmath::inverse; 37 | using libcgt::core::geometry::translate; 38 | 39 | namespace { 40 | void DebugCallback(GLenum source, GLenum type, GLuint id, GLenum severity, 41 | GLsizei length, const GLchar* message, const void* userParam) { 42 | if (severity != GL_DEBUG_SEVERITY_NOTIFICATION) { 43 | fprintf(stderr, "[GL DEBUG]: %s\n", message); 44 | } 45 | } 46 | } 47 | 48 | MainWidget::MainWidget(QWidget* parent) : 49 | QOpenGLWidget(parent) { 50 | QSurfaceFormat format = QSurfaceFormat::defaultFormat(); 51 | format.setProfile(QSurfaceFormat::CoreProfile); 52 | format.setMajorVersion(4); 53 | format.setMinorVersion(5); 54 | #if _DEBUG 55 | format.setOption(QSurfaceFormat::DebugContext, true); 56 | #endif 57 | setFormat(format); 58 | 59 | const float aspectRatio = static_cast(width()) / height(); 60 | free_camera_.setFrustum(GLFrustum::makeSymmetricPerspective( 61 | kFreeCameraFovYRadians, aspectRatio, kFreeCameraZNear, kFreeCameraZFar)); 62 | } 63 | 64 | void MainWidget::SetPipeline(RegularGridFusionPipeline* pipeline) { 65 | moving_pipeline_ = pipeline; 66 | } 67 | 68 | void MainWidget::SetPipeline(MultiStaticCameraPipeline* pipeline) { 69 | static_pipeline_ = pipeline; 70 | } 71 | 72 | SingleMovingCameraGLState* MainWidget::GetSingleMovingCameraGLState() const { 73 | return moving_gl_state_.get(); 74 | } 75 | 76 | MultiStaticCameraGLState* MainWidget::GetMultiStaticCameraGLState() const { 77 | return static_gl_state_.get(); 78 | } 79 | 80 | void MainWidget::initializeGL() { 81 | GLenum err = glewInit(); 82 | if (err != GLEW_OK) { 83 | fprintf(stderr, "Error initializing GLEW: %s\n", glewGetErrorString(err)); 84 | exit(1); 85 | } 86 | 87 | #if _DEBUG 88 | glEnable(GL_DEBUG_OUTPUT_SYNCHRONOUS); 89 | glDebugMessageCallback(DebugCallback, this); 90 | #endif 91 | 92 | glClearColor(0, 0, 0, 0); 93 | glEnable(GL_DEPTH_TEST); 94 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 95 | glBlendEquation(GL_FUNC_ADD); 96 | 97 | if (moving_pipeline_ != nullptr) { 98 | moving_gl_state_ = std::make_unique( 99 | moving_pipeline_, this); 100 | } 101 | 102 | if (static_pipeline_ != nullptr) { 103 | static_gl_state_ = std::make_unique( 104 | static_pipeline_, this); 105 | } 106 | } 107 | 108 | // virtual 109 | void MainWidget::keyPressEvent(QKeyEvent* e) { 110 | 111 | } 112 | 113 | void MainWidget::paintGL() { 114 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 115 | 116 | if (static_gl_state_ != nullptr) { 117 | static_gl_state_->Render(free_camera_); 118 | } 119 | 120 | if (moving_gl_state_ != nullptr) { 121 | moving_gl_state_->Render(free_camera_); 122 | } 123 | } 124 | 125 | void MainWidget::resizeGL(int w, int h) { 126 | glViewport(0, 0, w, h); 127 | 128 | float aspectRatio = static_cast(w) / h; 129 | free_camera_.setFrustum(GLFrustum::makeSymmetricPerspective( 130 | kFreeCameraFovYRadians, aspectRatio, kFreeCameraZNear, kFreeCameraZFar)); 131 | 132 | if (static_gl_state_ != nullptr) { 133 | static_gl_state_->Resize({w, h}); 134 | } 135 | 136 | if(moving_gl_state_ != nullptr) { 137 | moving_gl_state_->Resize({w, h}); 138 | } 139 | 140 | update(); 141 | } 142 | 143 | void MainWidget::mousePressEvent(QMouseEvent* e) { 144 | fps_controls_.handleMousePressEvent(e); 145 | update(); 146 | } 147 | 148 | void MainWidget::mouseMoveEvent(QMouseEvent* e) { 149 | fps_controls_.handleMouseMoveEvent(e, free_camera_); 150 | update(); 151 | } 152 | 153 | void MainWidget::mouseReleaseEvent(QMouseEvent* e) { 154 | fps_controls_.handleMouseReleaseEvent(e); 155 | update(); 156 | } 157 | -------------------------------------------------------------------------------- /src/main_widget.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MAIN_WIDGET_H 15 | #define MAIN_WIDGET_H 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "libcgt/core/cameras/PerspectiveCamera.h" 23 | #include "libcgt/core/common/Array2D.h" 24 | #include "libcgt/core/common/ArrayUtils.h" 25 | #include "libcgt/core/common/BasicTypes.h" 26 | #include "libcgt/qt_interop/FPSControls.h" 27 | 28 | #include "single_moving_camera_gl_state.h" 29 | #include "multi_static_camera_gl_state.h" 30 | 31 | class RegularGridFusionPipeline; 32 | class MultiStaticCameraPipeline; 33 | 34 | class MainWidget : public QOpenGLWidget { 35 | public: 36 | 37 | MainWidget(QWidget* parent = nullptr); 38 | 39 | void SetPipeline(RegularGridFusionPipeline* pipeline); 40 | void SetPipeline(MultiStaticCameraPipeline* pipeline); 41 | 42 | SingleMovingCameraGLState* GetSingleMovingCameraGLState() const; 43 | MultiStaticCameraGLState* GetMultiStaticCameraGLState() const; 44 | 45 | protected: 46 | virtual void initializeGL(); 47 | virtual void paintGL(); 48 | virtual void resizeGL(int w, int h); 49 | 50 | virtual void mousePressEvent(QMouseEvent* e) override; 51 | virtual void mouseMoveEvent(QMouseEvent* e) override; 52 | virtual void mouseReleaseEvent(QMouseEvent* e) override; 53 | 54 | virtual void keyPressEvent(QKeyEvent* e) override; 55 | 56 | private: 57 | 58 | RegularGridFusionPipeline* moving_pipeline_ = nullptr; 59 | std::unique_ptr moving_gl_state_ = nullptr; 60 | 61 | MultiStaticCameraPipeline* static_pipeline_ = nullptr; 62 | std::unique_ptr static_gl_state_ = nullptr; 63 | 64 | FPSControls fps_controls_; 65 | PerspectiveCamera free_camera_; 66 | }; 67 | 68 | #endif // MAIN_WIDGET_H 69 | -------------------------------------------------------------------------------- /src/marching_cubes.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MARCHING_CUBES_H 15 | #define MARCHING_CUBES_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/common/Array3D.h" 20 | #include "libcgt/core/vecmath/SimilarityTransform.h" 21 | #include "libcgt/core/vecmath/Vector3f.h" 22 | #include "libcgt/core/geometry/TriangleMesh.h" 23 | 24 | #include "tsdf.h" 25 | 26 | // Run the marching cubes algorithm on the regular grid TSDF. 27 | // Generates a triangle list of positions and normals. 28 | void MarchingCubes(Array3DReadView grid, float max_tsdf_value, 29 | const libcgt::core::vecmath::SimilarityTransform& world_from_grid, 30 | std::vector& triangle_list_positions_out, 31 | std::vector& triangle_list_normals_out); 32 | 33 | TriangleMesh ConstructMarchingCubesMesh( 34 | const std::vector& triangle_list_positions); 35 | 36 | TriangleMesh ConstructMarchingCubesMesh( 37 | const std::vector& triangle_list_positions, 38 | const std::vector& triangle_list_normals); 39 | 40 | #endif // MARCHING_CUBES_H 41 | -------------------------------------------------------------------------------- /src/multi_static_camera_gl_state.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MULTI_STATIC_CAMERA_GL_STATE_H 15 | #define MULTI_STATIC_CAMERA_GL_STATE_H 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "libcgt/core/common/Array2D.h" 24 | #include "libcgt/cuda/DeviceArray2D.h" 25 | #include "libcgt/cuda/gl/Texture2D.h" 26 | #include "libcgt/GL/GLProgramPipeline.h" 27 | #include "libcgt/GL/GLSamplerObject.h" 28 | #include "libcgt/GL/GLSeparableProgram.h" 29 | #include "libcgt/GL/GL_45/GLTexture2D.h" 30 | #include "libcgt/GL/GL_45/drawables/Axes.h" 31 | #include "libcgt/GL/GL_45/drawables/Frustum.h" 32 | #include "libcgt/GL/GL_45/drawables/PointCloud.h" 33 | #include "libcgt/GL/GL_45/drawables/TexturedRectangle.h" 34 | #include "libcgt/GL/GL_45/drawables/WireframeBox.h" 35 | 36 | class MultiStaticCameraPipeline; 37 | 38 | class MultiStaticCameraGLState : public QObject { 39 | 40 | Q_OBJECT 41 | 42 | public: 43 | MultiStaticCameraGLState(MultiStaticCameraPipeline* pipeline, 44 | QOpenGLWidget* parent); 45 | 46 | //void NotifyInputUpdated(); 47 | void NotifyTSDFUpdated(); 48 | 49 | void Resize(const Vector2i& size); 50 | void Render(const PerspectiveCamera& free_camera); 51 | 52 | private: 53 | 54 | // ----- Helper functions ----- 55 | void DrawInputsAndIntermediates(); 56 | void DrawWorldAxes(); 57 | void DrawCameraFrustaAndTSDFGrid(); 58 | void DrawUnprojectedPointClouds(); 59 | void DrawFullscreenRaycast(); 60 | 61 | // ----- State ----- 62 | QOpenGLWidget* parent_ = nullptr; 63 | MultiStaticCameraPipeline* pipeline_ = nullptr; 64 | bool tsdf_is_dirty_ = true; 65 | PerspectiveCamera free_camera_; 66 | Vector2i window_size_; 67 | 68 | // ----- Drawables ----- 69 | Axes world_axes_; 70 | TexturedRectangle input_buffer_textured_rect_; 71 | std::vector depth_camera_frusta_; 72 | WireframeBox tsdf_bbox_; 73 | PointCloud xy_coords_; 74 | 75 | // TODO: interop, avoid copy: 76 | // need to wrap a DeviceOpaqueArray, and then enable surface writes. 77 | std::vector raw_depth_textures_; 78 | std::vector undistorted_depth_textures_; 79 | 80 | // Raycasting from the current camera pose, resized as the view changes. 81 | DeviceArray2D free_camera_world_positions_; 82 | DeviceArray2D free_camera_world_normals_; 83 | libcgt::cuda::gl::Texture2D free_camera_world_positions_tex_; 84 | libcgt::cuda::gl::Texture2D free_camera_world_normals_tex_; 85 | 86 | // ----- GL helper objects ----- 87 | GLProgramPipeline draw_color_; 88 | GLProgramPipeline draw_single_color_; 89 | GLProgramPipeline draw_texture_; 90 | GLProgramPipeline unproject_point_cloud_; 91 | 92 | GLSamplerObject nearest_sampler_; 93 | GLSamplerObject linear_sampler_; 94 | }; 95 | 96 | #endif // MULTI_STATIC_CAMERA_GL_STATE_H 97 | -------------------------------------------------------------------------------- /src/multi_static_camera_pipeline.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "multi_static_camera_pipeline.h" 15 | 16 | #include 17 | 18 | #include 19 | 20 | #include "libcgt/core/common/ArrayUtils.h" 21 | #include "libcgt/cuda/VecmathConversions.h" 22 | #include "libcgt/cuda/VectorFunctions.h" 23 | 24 | using libcgt::core::arrayutils::cast; 25 | using libcgt::core::cameras::Intrinsics; 26 | using libcgt::core::vecmath::SimilarityTransform; 27 | 28 | DECLARE_bool(adaptive_raycast); 29 | 30 | MultiStaticCameraPipeline::MultiStaticCameraPipeline( 31 | const std::vector& camera_params, 32 | const std::vector& depth_camera_poses_cfw, 33 | const Vector3i& grid_resolution, 34 | const SimilarityTransform& world_from_grid, 35 | float max_tsdf_value) : 36 | regular_grid_(grid_resolution, world_from_grid, max_tsdf_value), 37 | 38 | camera_params_(camera_params), 39 | depth_camera_poses_cfw_(depth_camera_poses_cfw), 40 | 41 | depth_processor_(camera_params[0].depth.intrinsics, 42 | camera_params[0].depth.depth_range) { 43 | 44 | for (size_t i = 0; i < camera_params.size(); ++i) { 45 | depth_meters_.emplace_back(camera_params[i].depth.resolution); 46 | depth_camera_undistort_maps_.emplace_back( 47 | camera_params[i].depth.resolution); 48 | undistorted_depth_meters_.emplace_back(camera_params[i].depth.resolution); 49 | input_buffers_.emplace_back(camera_params[i].color.resolution, 50 | camera_params[i].depth.resolution); 51 | 52 | copy(cast(camera_params[i].depth.undistortion_map.readView()), 53 | depth_camera_undistort_maps_[i]); 54 | } 55 | } 56 | 57 | int MultiStaticCameraPipeline::NumCameras() const { 58 | return static_cast(camera_params_.size()); 59 | } 60 | 61 | const RGBDCameraParameters& MultiStaticCameraPipeline::GetCameraParameters( 62 | int camera_index ) const { 63 | return camera_params_[camera_index]; 64 | } 65 | 66 | Box3f MultiStaticCameraPipeline::TSDFGridBoundingBox() const { 67 | return regular_grid_.BoundingBox(); 68 | } 69 | 70 | const SimilarityTransform& 71 | MultiStaticCameraPipeline::TSDFWorldFromGridTransform() const { 72 | return regular_grid_.WorldFromGrid(); 73 | } 74 | 75 | void MultiStaticCameraPipeline::Reset() { 76 | regular_grid_.Reset(); 77 | } 78 | 79 | void MultiStaticCameraPipeline::NotifyInputUpdated(int camera_index, 80 | bool color_updated, 81 | bool depth_updated) { 82 | copy(input_buffers_[camera_index].depth_meters.readView(), 83 | depth_meters_[camera_index]); 84 | 85 | depth_processor_.Undistort( 86 | depth_meters_[camera_index], depth_camera_undistort_maps_[camera_index], 87 | undistorted_depth_meters_[camera_index]); 88 | } 89 | 90 | InputBuffer& MultiStaticCameraPipeline::GetInputBuffer(int camera_index) { 91 | return input_buffers_[camera_index]; 92 | } 93 | 94 | const DeviceArray2D& MultiStaticCameraPipeline::GetUndistortedDepthMap( 95 | int camera_index) { 96 | return undistorted_depth_meters_[camera_index]; 97 | } 98 | 99 | PerspectiveCamera MultiStaticCameraPipeline::GetDepthCamera( 100 | int camera_index) const { 101 | return PerspectiveCamera( 102 | depth_camera_poses_cfw_[camera_index], 103 | camera_params_[camera_index].depth.intrinsics, 104 | Vector2f(camera_params_[camera_index].depth.resolution), 105 | camera_params_[camera_index].depth.depth_range.left(), 106 | camera_params_[camera_index].depth.depth_range.right() 107 | ); 108 | } 109 | 110 | void MultiStaticCameraPipeline::Fuse() { 111 | // Right now, fuse them all, time aligned. 112 | // sweep over all the ones that are ready. 113 | 114 | // TODO: instead of N sweeps over the volume, for each voxel, can sweep 115 | // over cameras instead. 116 | for (size_t i = 0; i < depth_meters_.size(); ++i) { 117 | Vector4f flpp = { 118 | camera_params_[i].depth.intrinsics.focalLength, 119 | camera_params_[i].depth.intrinsics.principalPoint 120 | }; 121 | regular_grid_.Fuse( 122 | flpp, camera_params_[i].depth.depth_range, 123 | depth_camera_poses_cfw_[i].asMatrix(), 124 | undistorted_depth_meters_[i] 125 | ); 126 | } 127 | } 128 | 129 | void MultiStaticCameraPipeline::FuseMultiple() { 130 | std::vector c(3); 131 | for (size_t i = 0; i < depth_meters_.size(); ++i) { 132 | c[i].flpp = make_float4( 133 | make_float2(camera_params_[i].depth.intrinsics.focalLength), 134 | make_float2(camera_params_[i].depth.intrinsics.principalPoint) 135 | ); 136 | c[i].depth_min_max = make_float2( 137 | camera_params_[i].depth.depth_range.leftRight() 138 | ); 139 | c[i].camera_from_world = make_float4x4( 140 | depth_camera_poses_cfw_[i].asMatrix() 141 | ); 142 | } 143 | 144 | regular_grid_.FuseMultiple(c, undistorted_depth_meters_); 145 | } 146 | 147 | void MultiStaticCameraPipeline::Raycast(const PerspectiveCamera& camera, 148 | DeviceArray2D& world_points, 149 | DeviceArray2D& world_normals) { 150 | Intrinsics intrinsics = camera.intrinsics(Vector2f(world_points.size())); 151 | Vector4f flpp{ intrinsics.focalLength, intrinsics.principalPoint }; 152 | 153 | if (FLAGS_adaptive_raycast) { 154 | regular_grid_.AdaptiveRaycast( 155 | flpp, 156 | camera.worldFromCamera().asMatrix(), 157 | world_points, 158 | world_normals 159 | ); 160 | } else { 161 | regular_grid_.Raycast( 162 | flpp, 163 | camera.worldFromCamera().asMatrix(), 164 | world_points, 165 | world_normals 166 | ); 167 | } 168 | } 169 | 170 | TriangleMesh MultiStaticCameraPipeline::Triangulate( 171 | const Matrix4f& output_from_world) const { 172 | TriangleMesh mesh = regular_grid_.Triangulate(); 173 | 174 | for (Vector3f& v : mesh.positions()) { 175 | v = output_from_world.transformPoint(v); 176 | } 177 | 178 | for (Vector3f& n : mesh.normals()) { 179 | n = output_from_world.transformNormal(n); 180 | } 181 | 182 | return mesh; 183 | } 184 | -------------------------------------------------------------------------------- /src/multi_static_camera_pipeline.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MULTI_STATIC_CAMERA_PIPELINE_H 15 | #define MULTI_STATIC_CAMERA_PIPELINE_H 16 | 17 | #include "libcgt/core/cameras/PerspectiveCamera.h" 18 | #include "libcgt/core/geometry/TriangleMesh.h" 19 | #include "libcgt/core/vecmath/Vector2i.h" 20 | #include "libcgt/core/vecmath/Vector3i.h" 21 | #include "libcgt/core/vecmath/EuclideanTransform.h" 22 | #include "libcgt/cuda/DeviceArray1D.h" 23 | #include "libcgt/cuda/DeviceArray2D.h" 24 | #include "libcgt/cuda/float4x4.h" 25 | 26 | #include "calibrated_posed_depth_camera.h" 27 | #include "depth_processor.h" 28 | #include "input_buffer.h" 29 | #include "pose_frame.h" 30 | #include "projective_point_plane_icp.h" 31 | #include "regular_grid_tsdf.h" 32 | #include "rgbd_camera_parameters.h" 33 | 34 | class MultiStaticCameraPipeline { 35 | 36 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 37 | using SimilarityTransform = libcgt::core::vecmath::SimilarityTransform; 38 | 39 | public: 40 | 41 | MultiStaticCameraPipeline( 42 | const std::vector& camera_params, 43 | const std::vector& depth_camera_poses_cfw, 44 | const Vector3i& grid_resolution, 45 | const SimilarityTransform& world_from_grid, 46 | float max_tsdf_value); 47 | 48 | int NumCameras() const; 49 | 50 | const RGBDCameraParameters& GetCameraParameters(int camera_index) const; 51 | 52 | // TODO: oriented box class 53 | Box3f TSDFGridBoundingBox() const; 54 | const SimilarityTransform& TSDFWorldFromGridTransform() const; 55 | 56 | void Reset(); 57 | 58 | void NotifyInputUpdated(int camera_index, 59 | bool color_updated, bool depth_updated); 60 | 61 | InputBuffer& GetInputBuffer(int camera_index); 62 | 63 | // Modified from the input... 64 | const DeviceArray2D& GetUndistortedDepthMap(int camera_index); 65 | 66 | PerspectiveCamera GetDepthCamera(int camera_index) const; 67 | 68 | // Update the regular grid with the latest image. 69 | void Fuse(); 70 | 71 | void FuseMultiple(); 72 | 73 | void Raycast(const PerspectiveCamera& camera, 74 | DeviceArray2D& world_points, 75 | DeviceArray2D& world_normals); 76 | 77 | TriangleMesh Triangulate( 78 | const Matrix4f& output_from_world = Matrix4f::identity()) const; 79 | 80 | private: 81 | // ----- Inputs ----- 82 | std::vector input_buffers_; 83 | 84 | // ----- Intermediate buffers ----- 85 | // Incoming raw depth frame in meters. 86 | std::vector> depth_meters_; 87 | // Incoming raw depth, undistorted. 88 | std::vector> undistorted_depth_meters_; 89 | 90 | // ----- Data structure to store the TSDF ----- 91 | RegularGridTSDF regular_grid_; 92 | 93 | // ----- Processors ----- 94 | DepthProcessor depth_processor_; 95 | 96 | // ----- Constants ----- 97 | const std::vector depth_camera_poses_cfw_; 98 | const std::vector camera_params_; 99 | std::vector> depth_camera_undistort_maps_; 100 | }; 101 | 102 | #endif // MULTI_STATIC_CAMERA_PIPELINE_H 103 | -------------------------------------------------------------------------------- /src/pipeline_data_type.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef PIPELINE_DATA_TYPE_H 15 | #define PIPELINE_DATA_TYPE_H 16 | 17 | #include 18 | 19 | enum class PipelineDataType { 20 | NONE = 0, 21 | INPUT_COLOR = 1, 22 | INPUT_DEPTH = 2, 23 | SMOOTHED_DEPTH = 4, 24 | CAMERA_POSE = 8, 25 | POSE_ESTIMATION_VIS = 16, 26 | TSDF = 32, 27 | RAYCAST_NORMALS = 64 28 | }; 29 | 30 | template<> 31 | struct enable_bitmask_operators { 32 | static const bool enable = true; 33 | }; 34 | 35 | #endif // PIPELINE_DATA_TYPE_H 36 | -------------------------------------------------------------------------------- /src/pose_estimation_method.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef POSE_ESTIMATION_METHOD_H 15 | #define POSE_ESTIMATION_METHOD_H 16 | 17 | #include 18 | 19 | enum class PoseEstimationMethod : uint32_t 20 | { 21 | NONE = 0, 22 | 23 | PRECOMPUTED = 1, 24 | PRECOMPUTED_REFINE_WITH_DEPTH_ICP = 2, 25 | 26 | COLOR_ARUCO = 32, 27 | 28 | DEPTH_ICP = 128, 29 | 30 | COLOR_ARUCO_AND_DEPTH_ICP = 256 31 | }; 32 | 33 | #endif // POSE_ESTIMATION_METHOD_H 34 | -------------------------------------------------------------------------------- /src/pose_frame.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef POSE_FRAME_H 15 | #define POSE_FRAME_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/vecmath/EuclideanTransform.h" 20 | 21 | struct PoseFrame { 22 | 23 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 24 | 25 | int32_t frame_index = 0; 26 | int64_t timestamp_ns = 0; 27 | 28 | EuclideanTransform color_camera_from_world; 29 | EuclideanTransform depth_camera_from_world; 30 | }; 31 | 32 | #endif // POSE_FRAME_H 33 | -------------------------------------------------------------------------------- /src/pose_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "pose_utils.h" 15 | 16 | #include "libcgt/camera_wrappers/PoseStream.h" 17 | #include "libcgt/core/vecmath/EuclideanTransform.h" 18 | 19 | using libcgt::camera_wrappers::PoseInputStream; 20 | using libcgt::camera_wrappers::PoseOutputStream; 21 | using libcgt::camera_wrappers::PoseStreamFormat; 22 | using libcgt::camera_wrappers::PoseStreamMetadata; 23 | using libcgt::camera_wrappers::PoseStreamTransformDirection; 24 | using libcgt::camera_wrappers::PoseStreamUnits; 25 | using libcgt::core::vecmath::EuclideanTransform; 26 | using libcgt::core::vecmath::transformPoint; 27 | 28 | std::vector LoadPoseHistory(const std::string& filename, 29 | const EuclideanTransform& depth_from_color) { 30 | std::vector output; 31 | 32 | // TODO: read other encodings properly, read the metadata, etc. 33 | PoseInputStream input_stream(filename.c_str()); 34 | if (input_stream.isValid()) { 35 | PoseFrame p; 36 | while (input_stream.read(p.frame_index, p.timestamp_ns, 37 | p.color_camera_from_world.rotation, 38 | p.color_camera_from_world.translation)) { 39 | p.depth_camera_from_world = depth_from_color * p.color_camera_from_world; 40 | output.push_back(p); 41 | } 42 | } 43 | 44 | return output; 45 | } 46 | 47 | bool SavePoseHistory(const std::vector& poses, 48 | const std::string& filename) { 49 | PoseStreamMetadata metadata; 50 | metadata.format = PoseStreamFormat::ROTATION_MATRIX_3X3_COL_MAJOR_AND_TRANSLATION_VECTOR_FLOAT; 51 | metadata.units = PoseStreamUnits::METERS; 52 | metadata.direction = PoseStreamTransformDirection::CAMERA_FROM_WORLD; 53 | 54 | PoseOutputStream output_stream(metadata, filename.c_str()); 55 | 56 | for( size_t i = 0; i < poses.size(); ++i ) 57 | { 58 | bool succeeded = output_stream.write( 59 | poses[i].frame_index, poses[ i ].timestamp_ns, 60 | poses[i].color_camera_from_world.rotation, 61 | poses[i].color_camera_from_world.translation 62 | ); 63 | if( !succeeded ) { 64 | return false; 65 | } 66 | } 67 | return true; 68 | } 69 | -------------------------------------------------------------------------------- /src/pose_utils.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef POSE_UTILS_H 15 | #define POSE_UTILS_H 16 | 17 | #include 18 | #include 19 | 20 | #include "libcgt/core/vecmath/EuclideanTransform.h" 21 | 22 | #include "pose_frame.h" 23 | 24 | std::vector LoadPoseHistory(const std::string& filename, 25 | const libcgt::core::vecmath::EuclideanTransform& depth_from_color); 26 | 27 | bool SavePoseHistory(const std::vector& pose_history, 28 | const std::string& filename); 29 | 30 | #endif // POSE_UTILS_H 31 | -------------------------------------------------------------------------------- /src/projective_point_plane_icp.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef PROJECTIVE_POINT_PLANE_ICP_H 15 | #define PROJECTIVE_POINT_PLANE_ICP_H 16 | 17 | #include "libcgt/core/cameras/Intrinsics.h" 18 | #include "libcgt/core/vecmath/EuclideanTransform.h" 19 | #include "libcgt/core/vecmath/Range1f.h" 20 | #include "libcgt/core/vecmath/Vector4f.h" 21 | #include "libcgt/cuda/DeviceArray2D.h" 22 | 23 | #include "icp_least_squares_data.h" 24 | 25 | #include 26 | 27 | class ProjectivePointPlaneICP { 28 | public: 29 | 30 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 31 | using Intrinsics = libcgt::core::cameras::Intrinsics; 32 | 33 | struct Result { 34 | bool valid = false; 35 | // The number of samples used to estimate this result. 36 | // If is 0, ICP failed and this result is invalid. 37 | int num_samples = 0; 38 | 39 | // TODO: reason: not enough matches, failed to translation and rotation tests, etc 40 | 41 | EuclideanTransform world_from_camera; 42 | }; 43 | 44 | ProjectivePointPlaneICP(const Vector2i& depth_resolution, 45 | const Intrinsics& depth_intrinsics, const Range1f& depth_range); 46 | 47 | __host__ 48 | Result EstimatePose( 49 | DeviceArray2D& incoming_depth, 50 | DeviceArray2D& incoming_normals, 51 | const EuclideanTransform& world_from_camera, 52 | DeviceArray2D& world_points, 53 | DeviceArray2D& world_normals, 54 | DeviceArray2D& debug_vis); 55 | 56 | private: 57 | 58 | const Vector4f depth_intrinsics_flpp_; 59 | const Range1f depth_range_; 60 | 61 | //const int kMinNumSamples = 30000; 62 | const int kMinNumSamples = 300; 63 | const int kNumIterations = 15; 64 | const int kImageGuardBand = 16; 65 | const float kMaxDistanceForMatch = 0.1f; 66 | const float kMinDotProductForMatch = 0.7f; 67 | 68 | // Reject if translation > kMaxTranslation meters; 69 | const float kMaxTranslation = 0.15f; 70 | // Reject if rotation > kMaxRotationRadians; 71 | const float kMaxRotationRadians = 0.1745f; // 10 degrees 72 | 73 | DeviceArray2D icp_data_; 74 | }; 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /src/raycast.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef RAYCAST_H 15 | #define RAYCAST_H 16 | 17 | #include 18 | 19 | #include "libcgt/cuda/KernelArray2D.h" 20 | #include "libcgt/cuda/KernelArray3D.h" 21 | #include "libcgt/cuda/float3x3.h" 22 | #include "libcgt/cuda/float4x4.h" 23 | 24 | #include "regular_grid_tsdf.h" 25 | 26 | __global__ 27 | void RaycastKernel(KernelArray3D regular_grid, 28 | float4x4 grid_from_world, // in meters 29 | float4x4 world_from_grid, // in meters 30 | float max_tsdf_value, 31 | float4 flpp, // camera intrinsics 32 | float4x4 world_from_camera, // camera pose 33 | float3 eye_world, // camera eye in world coords 34 | KernelArray2D world_depth_out, 35 | KernelArray2D world_normal_out 36 | ); 37 | 38 | __global__ 39 | void AdaptiveRaycastKernel(KernelArray3D regular_grid, 40 | float4x4 grid_from_world, // in meters 41 | float4x4 world_from_grid, // in meters 42 | float max_tsdf_value, 43 | float voxels_per_meter, 44 | float4 flpp, // camera intrinsics 45 | float4x4 world_from_camera, // camera pose 46 | float3 eye_world, // camera eye in world coords 47 | KernelArray2D world_depth_out, 48 | KernelArray2D world_normal_out 49 | ); 50 | 51 | #endif // RAYCAST_H 52 | -------------------------------------------------------------------------------- /src/raycast_volume/raycast_volume_cli.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | 16 | #include 17 | #include "libcgt/camera_wrappers/PoseStream.h" 18 | #include "libcgt/core/common/ArrayUtils.h" 19 | #include "libcgt/core/common/StringUtils.h" 20 | #include "libcgt/core/io/PortableFloatMapIO.h" 21 | #include "libcgt/core/vecmath/EuclideanTransform.h" 22 | #include "third_party/pystring/pystring.h" 23 | 24 | #include "../pose_frame.h" 25 | #include "../pose_utils.h" 26 | #include "../regular_grid_tsdf.h" 27 | #include "../rgbd_camera_parameters.h" 28 | 29 | // Options. 30 | DEFINE_bool(collect_perf, false, "Collect performance statistics."); 31 | 32 | // Inputs. 33 | DEFINE_string(tsdf3d, "", "Input TSDF"); 34 | DEFINE_string(intrinsics, "", "Input camera intrinsics (.yaml)"); 35 | DEFINE_string(pose, "", "Input camera path (.pose)"); 36 | 37 | // Outputs. 38 | // TODO: use undistorted intrinsics. 39 | DEFINE_string(output_dir, "", "Output directory"); 40 | DEFINE_bool(output_depth, false, "Output depth"); // units are meters 41 | DEFINE_bool(output_world_points, true, "Output world points"); 42 | DEFINE_bool(output_world_normals, true, "Output world normals"); 43 | // TODO: secondary bounce points and normals. 44 | // TODO: adaptive raycast 45 | // TODO: get rid of these 46 | 47 | using libcgt::camera_wrappers::PoseInputStream; 48 | using libcgt::camera_wrappers::PoseStreamFormat; 49 | using libcgt::camera_wrappers::PoseStreamMetadata; 50 | using libcgt::camera_wrappers::PoseStreamTransformDirection; 51 | using libcgt::camera_wrappers::PoseStreamUnits; 52 | using libcgt::core::cameras::Intrinsics; 53 | using libcgt::core::arrayutils::cast; 54 | using libcgt::core::arrayutils::flipY; 55 | using libcgt::core::stringPrintf; 56 | using libcgt::core::vecmath::EuclideanTransform; 57 | using libcgt::core::vecmath::SimilarityTransform; 58 | using pystring::os::path::join; 59 | 60 | struct TimestampedPose { 61 | int32_t frame_index; 62 | int64_t timestamp; 63 | EuclideanTransform camera_from_world; 64 | }; 65 | 66 | int main(int argc, char* argv[]) { 67 | gflags::ParseCommandLineFlags(&argc, &argv, true); 68 | 69 | if (FLAGS_output_dir.empty()) { 70 | fprintf(stderr, "output_dir is required.\n"); 71 | return 1; 72 | } 73 | 74 | if (!FLAGS_output_depth && 75 | !FLAGS_output_world_points && 76 | !FLAGS_output_world_normals) { 77 | fprintf(stderr, "Must specify at least one output.\n"); 78 | return 1; 79 | } 80 | 81 | if (FLAGS_output_depth) { 82 | fprintf(stderr, "Depth output not yet implemented.\n"); 83 | return 1; 84 | } 85 | 86 | // Load intrinsics. 87 | CameraParameters camera_params; 88 | if (!LoadCameraParameters(FLAGS_intrinsics, &camera_params)) { 89 | fprintf(stderr, "Failed to read camera intrinsics.\n"); 90 | return 1; 91 | } 92 | 93 | // TODO: implement load as a free function without a dummy constructor. 94 | // currently, we're stuck at 512^3!. 95 | Vector3i resolution{512}; 96 | RegularGridTSDF tsdf(resolution, SimilarityTransform{}); 97 | 98 | if (!tsdf.Load(FLAGS_tsdf3d)) { 99 | fprintf(stderr, "Error loading TSDF3D from %s\n.", 100 | FLAGS_tsdf3d.c_str()); 101 | return 1; 102 | } 103 | 104 | std::vector camera_path; 105 | 106 | int32_t frame_index; 107 | int64_t timestamp; 108 | EuclideanTransform e; 109 | 110 | // TODO: make a class and function for loading a camera path with intrinsics. 111 | PoseInputStream pose_stream(FLAGS_pose.c_str()); 112 | bool ok = pose_stream.read(frame_index, timestamp, 113 | e.rotation, e.translation); 114 | while (ok) { 115 | if (pose_stream.metadata().direction == 116 | PoseStreamTransformDirection::WORLD_FROM_CAMERA) { 117 | camera_path.push_back( 118 | TimestampedPose{frame_index, timestamp, inverse(e)}); 119 | } else { 120 | camera_path.push_back(TimestampedPose{frame_index, timestamp, e}); 121 | } 122 | ok = pose_stream.read(frame_index, timestamp, e.rotation, e.translation); 123 | } 124 | 125 | if (camera_path.empty()) { 126 | fprintf(stderr, "Error loading camera poses from %s\n.", 127 | FLAGS_pose.c_str()); 128 | } 129 | 130 | DeviceArray2D world_points(camera_params.resolution); 131 | DeviceArray2D world_normals(camera_params.resolution); 132 | Array2D host_world_points(camera_params.resolution); 133 | Array2D host_world_normals(camera_params.resolution); 134 | 135 | const Vector4f flpp{camera_params.undistorted_intrinsics.focalLength, 136 | camera_params.undistorted_intrinsics.principalPoint}; 137 | 138 | for (size_t i = 0; i < camera_path.size(); ++i) { 139 | printf("Raycasting frame %zu of %zu\n", i, camera_path.size()); 140 | 141 | const auto& pose = camera_path[i]; 142 | tsdf.Raycast(flpp, inverse(pose.camera_from_world).asMatrix(), 143 | world_points, world_normals); 144 | 145 | if (FLAGS_output_world_points || FLAGS_output_depth) { 146 | copy(world_points, cast(host_world_points.writeView())); 147 | 148 | std::string world_points_filename = 149 | stringPrintf("world_points_%05d_%020lld.pfm4", 150 | pose.frame_index, pose.timestamp); 151 | PortableFloatMapIO::write(flipY(host_world_points.readView()), 152 | join(FLAGS_output_dir, world_points_filename)); 153 | } 154 | if (FLAGS_output_world_normals) { 155 | copy(world_normals, cast(host_world_normals.writeView())); 156 | 157 | std::string world_normals_filename = 158 | stringPrintf("world_normals_%05d_%020lld.pfm4", 159 | pose.frame_index, pose.timestamp); 160 | PortableFloatMapIO::write(flipY(host_world_normals.readView()), 161 | join(FLAGS_output_dir, world_normals_filename)); 162 | } 163 | } 164 | 165 | return 0; 166 | } 167 | -------------------------------------------------------------------------------- /src/regular_grid_fusion_pipeline.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REGULAR_GRID_FUSION_PIPELINE_H 15 | #define REGULAR_GRID_FUSION_PIPELINE_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/cameras/PerspectiveCamera.h" 20 | #include "libcgt/core/geometry/TriangleMesh.h" 21 | #include "libcgt/core/vecmath/Vector2i.h" 22 | #include "libcgt/core/vecmath/Vector3i.h" 23 | #include "libcgt/core/vecmath/EuclideanTransform.h" 24 | #include "libcgt/cuda/DeviceArray2D.h" 25 | 26 | #include "aruco/aruco_pose_estimator.h" 27 | #include "aruco/cube_fiducial.h" 28 | #include "aruco/single_marker_fiducial.h" 29 | #include "regular_grid_tsdf.h" 30 | #include "rgbd_camera_parameters.h" 31 | #include "depth_processor.h" 32 | #include "input_buffer.h" 33 | #include "pipeline_data_type.h" 34 | #include "pose_estimation_method.h" 35 | #include "pose_frame.h" 36 | #include "projective_point_plane_icp.h" 37 | 38 | struct PoseEstimatorOptions { 39 | PoseEstimationMethod method = 40 | PoseEstimationMethod::COLOR_ARUCO_AND_DEPTH_ICP; 41 | 42 | // TODO: move fiducial here. 43 | 44 | // Required if method is DEPTH_ICP. 45 | PoseFrame initial_pose = PoseFrame{}; 46 | 47 | // Required if method is PRECOMPUTED or PRECOMPUTED_REFINE_WITH_DEPTH_ICP. 48 | std::vector precomputed_path; 49 | }; 50 | 51 | class RegularGridFusionPipeline : public QObject { 52 | Q_OBJECT 53 | 54 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 55 | using SimilarityTransform = libcgt::core::vecmath::SimilarityTransform; 56 | 57 | public: 58 | 59 | RegularGridFusionPipeline( 60 | const RGBDCameraParameters& camera_params, 61 | const Vector3i& grid_resolution, 62 | const SimilarityTransform& world_from_grid, 63 | const PoseEstimatorOptions& pose_estimator_options); 64 | 65 | // TODO: refactor this. 66 | bool LoadTSDF3D(const std::string& filename); 67 | bool SaveTSDF3D(const std::string& filename) const; 68 | 69 | void Reset(); 70 | 71 | const RGBDCameraParameters& GetCameraParameters() const; 72 | const CubeFiducial& GetArucoCubeFiducial() const; 73 | const SingleMarkerFiducial& GetArucoSingleMarkerFiducial() const; 74 | 75 | // Get a mutable reference to the input buffer. Clients should update 76 | // fields of the input buffer then call NotifyColorUpdated() or 77 | // NotifyDepthUpdated() to trigger a computation. 78 | InputBuffer& GetInputBuffer(); 79 | 80 | // Get a read-only view of the latest color pose estimator's visualization. 81 | // TODO: this buffer is y-up but BGR format. 82 | Array2DReadView GetColorPoseEstimatorVisualization() const; 83 | 84 | void NotifyColorUpdated(); 85 | 86 | void NotifyDepthUpdated(); 87 | 88 | // Returns the TSDF grid's axis aligned bounding box. 89 | // (0, 0, 0) --> Resolution(). 90 | // TODO: implement a simple oriented box class. 91 | Box3f TSDFGridBoundingBox() const; 92 | 93 | // The transformation that yields world coordinates (in meters) from 94 | // grid coordinates [0, resolution]^3 (in samples). 95 | const SimilarityTransform& TSDFWorldFromGridTransform() const; 96 | 97 | // TODO: get rid of these. 98 | PerspectiveCamera ColorCamera() const; 99 | PerspectiveCamera DepthCamera() const; 100 | 101 | // Update the regular grid with the latest image. 102 | void Fuse(); 103 | 104 | // Update world_points_ and world_normals_ by raycasting the latest regular 105 | // grid from the current pose. 106 | void Raycast(); 107 | 108 | void Raycast(const PerspectiveCamera& camera, 109 | DeviceArray2D& world_points, 110 | DeviceArray2D& world_normals); 111 | 112 | TriangleMesh Triangulate() const; 113 | 114 | // Returns CameraFromworld. 115 | const std::vector& PoseHistory() const; 116 | 117 | const DeviceArray2D& SmoothedDepthMeters() const; 118 | 119 | // In camera space. 120 | const DeviceArray2D& SmoothedIncomingNormals() const; 121 | 122 | const DeviceArray2D& PoseEstimationVisualization() const; 123 | 124 | // In world space. 125 | const DeviceArray2D& RaycastNormals() const; 126 | 127 | signals: 128 | 129 | // Notify subscribers that data flowing through the pipeline has changed. 130 | void dataChanged(PipelineDataType type); 131 | 132 | private: 133 | 134 | // Try to estimate the rgbd camera pose using the latest color frame of the 135 | // pipeline's input buffer. If it succeeded, it will be appended to the 136 | // pose history and returns true. Otherwise, returns false. 137 | bool UpdatePoseWithColorCamera(); 138 | 139 | // Try to estimate the rgbd camera pose using the latest depth frame of the 140 | // pipeline's input buffer. If it succeeded, returns true and writes the 141 | // result in pose_frame_out. Otherwise, returns false. 142 | bool UpdatePoseWithDepthCamera(PoseFrame* pose_frame_out); 143 | 144 | // CPU input buffers. 145 | InputBuffer input_buffer_; 146 | 147 | // ----- Input copied to the GPU ----- 148 | // Incoming depth frame in meters. 149 | DeviceArray2D depth_meters_; 150 | 151 | // ----- Pipeline intermediates ----- 152 | 153 | // Incoming depth smoothed using a bilateral filter. 154 | DeviceArray2D smoothed_depth_meters_; 155 | // Incoming camera-space normals, estimated from smoothed depth. 156 | DeviceArray2D incoming_camera_normals_; 157 | 158 | // Pose estimation visualization. 159 | DeviceArray2D pose_estimation_vis_; 160 | 161 | // Raycasted world-space points and normals. 162 | PoseFrame last_raycast_pose_ = {}; 163 | DeviceArray2D world_points_; 164 | DeviceArray2D world_normals_; 165 | 166 | DepthProcessor depth_processor_; 167 | 168 | RegularGridTSDF regular_grid_; 169 | 170 | // TODO: consider removing this. 171 | const int kMaxSuccessiveFailuresBeforeReset = 1000; 172 | int num_successive_failures_ = 0; 173 | ProjectivePointPlaneICP icp_; 174 | 175 | CubeFiducial aruco_cube_fiducial_; 176 | SingleMarkerFiducial aruco_single_marker_fiducial_; 177 | ArucoPoseEstimator aruco_pose_estimator_; 178 | // Visualization of the last pose estimate, y up. 179 | Array2D aruco_vis_; 180 | 181 | PoseEstimatorOptions pose_estimator_options_; 182 | bool is_first_depth_frame_ = true; 183 | std::vector pose_history_; 184 | 185 | const RGBDCameraParameters camera_params_; 186 | // camera_params_.depth_intrinsics_, stored as a Vector4f. 187 | const Vector4f depth_intrinsics_flpp_; 188 | const Range1f depth_range_; 189 | }; 190 | 191 | #endif // REGULAR_GRID_FUSION_PIPELINE_H 192 | -------------------------------------------------------------------------------- /src/regular_grid_tsdf.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef REGULAR_GRID_TSDF_H 15 | #define REGULAR_GRID_TSDF_H 16 | 17 | #include "libcgt/core/geometry/TriangleMesh.h" 18 | #include "libcgt/core/vecmath/Box3f.h" 19 | #include "libcgt/core/vecmath/Matrix4f.h" 20 | #include "libcgt/core/vecmath/Range1f.h" 21 | #include "libcgt/core/vecmath/SimilarityTransform.h" 22 | #include "libcgt/core/vecmath/Vector3i.h" 23 | #include "libcgt/core/vecmath/Vector4f.h" 24 | #include "libcgt/cuda/DeviceArray1D.h" 25 | #include "libcgt/cuda/DeviceArray2D.h" 26 | #include "libcgt/cuda/DeviceArray3D.h" 27 | 28 | #include "calibrated_posed_depth_camera.h" 29 | #include 30 | #include "tsdf.h" 31 | 32 | class RegularGridTSDF { 33 | 34 | using SimilarityTransform = libcgt::core::vecmath::SimilarityTransform; 35 | 36 | public: 37 | 38 | // Same as RegularGridTSDF(resolution, world_from_grid, 4 * VoxelSize()). 39 | RegularGridTSDF(const Vector3i& resolution, 40 | const SimilarityTransform& world_from_grid); 41 | 42 | // resolution: number of voxels in each direction. 43 | // world_from_grid: transform mapping grid indices to world coordinates 44 | // world_from_grid.scale is the side length of one (cubical) voxel in world 45 | // units. 46 | // max_tsdf_value: the representable range of the TSDF. Set to 47 | // [-max_tsdf_value, max_tsdf_value]. 48 | RegularGridTSDF(const Vector3i& resolution, 49 | const SimilarityTransform& world_from_grid, 50 | float max_tsdf_value); 51 | 52 | void Reset(); 53 | 54 | void Fuse(const Vector4f& depth_camera_flpp, // Depth camera intrinsics. 55 | const Range1f& depth_camera_range, // Depth camera range. 56 | const Matrix4f& depth_camera_from_world, // Depth camera pose. 57 | const DeviceArray2D& depth_data); // Depth frame, in meters. 58 | 59 | void FuseMultiple( 60 | const std::vector& depth_cameras, 61 | const std::vector>& depth_maps); 62 | 63 | void AdaptiveRaycast( const Vector4f& camera_flpp, // Camera intrinsics 64 | const Matrix4f& world_from_camera, // Camera pose. 65 | DeviceArray2D& world_points_out, 66 | DeviceArray2D& world_normals_out ); 67 | 68 | void Raycast(const Vector4f& camera_flpp, // Camera intrinsics 69 | const Matrix4f& world_from_camera, // Camera pose. 70 | DeviceArray2D& world_points_out, 71 | DeviceArray2D& world_normals_out); 72 | 73 | // The transformation that yields grid coordinates [0, resolution]^3 (in 74 | // samples), from world coordinates (in meters). 75 | const SimilarityTransform& GridFromWorld() const; 76 | 77 | // The transformation that yields world coordinates (in meters) from 78 | // grid coordinates [0, resolution]^3 (in samples). 79 | const SimilarityTransform& WorldFromGrid() const; 80 | 81 | // (0, 0, 0) --> Resolution(). 82 | Box3f BoundingBox() const; 83 | 84 | // The number of samples of the grid along each axis. 85 | Vector3i Resolution() const; 86 | 87 | // The side length of one (cubical) voxel, in meters. 88 | float VoxelSize() const; 89 | 90 | // The side lengths of the entire grid, in meters. 91 | // Equivalent to VoxelSize() * Resolution(). 92 | Vector3f SideLengths() const; 93 | 94 | TriangleMesh Triangulate() const; 95 | 96 | bool Load(const std::string& filename); 97 | bool Save(const std::string& filename) const; 98 | 99 | private: 100 | 101 | SimilarityTransform grid_from_world_; 102 | SimilarityTransform world_from_grid_; 103 | 104 | DeviceArray3D device_grid_; 105 | 106 | // TODO: this should be dynamic, and is a function of the noise model. 107 | float max_tsdf_value_; 108 | }; 109 | 110 | #endif // REGULAR_GRID_TSDF_H 111 | -------------------------------------------------------------------------------- /src/rgbd_camera_parameters.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "rgbd_camera_parameters.h" 15 | 16 | #include "libcgt/core/io/PortableFloatMapIO.h" 17 | #include "libcgt/core/vecmath/EuclideanTransform.h" 18 | #include "libcgt/opencv_interop/Calib3d.h" 19 | #include "libcgt/opencv_interop/VecmathUtils.h" 20 | #include 21 | 22 | using namespace pystring; 23 | using namespace libcgt::core::vecmath; 24 | 25 | using libcgt::opencv_interop::cameraMatrixToIntrinsics; 26 | 27 | namespace { 28 | 29 | CameraParameters LoadCameraIntrinsics(const cv::FileStorage& fs, 30 | const std::string& namePrefix) { 31 | CameraParameters params = {}; 32 | 33 | cv::Size image_size; 34 | cv::Mat intrinsics_matrix_gl; 35 | cv::Mat dist_coeffs; 36 | cv::Mat undistorted_intrinsics_matrix_gl; 37 | 38 | fs[namePrefix + "ImageSize"] >> image_size; 39 | fs[namePrefix + "CameraMatrix_gl"] >> intrinsics_matrix_gl; 40 | fs[namePrefix + "DistCoeffs"] >> dist_coeffs; 41 | fs[namePrefix + "NewCameraMatrix_gl" ] >> undistorted_intrinsics_matrix_gl; 42 | 43 | params.resolution = { image_size.width, image_size.height }; 44 | 45 | params.intrinsics = cameraMatrixToIntrinsics(intrinsics_matrix_gl); 46 | for (int i = 0; i < 5; ++i) { 47 | params.dist_coeffs.push_back( 48 | static_cast(dist_coeffs.at(0, i))); 49 | } 50 | 51 | params.undistorted_intrinsics = cameraMatrixToIntrinsics( 52 | undistorted_intrinsics_matrix_gl); 53 | 54 | return params; 55 | } 56 | 57 | } // namespace 58 | 59 | EuclideanTransform LoadEuclideanTransform(const cv::FileStorage& fs, 60 | const std::string& namePrefix) { 61 | cv::Mat r; 62 | cv::Mat t; 63 | 64 | fs[namePrefix + "_R"] >> r; 65 | fs[namePrefix + "_T"] >> t; 66 | 67 | return EuclideanTransform 68 | { 69 | libcgt::opencv_interop::fromCV3x3(r), 70 | libcgt::opencv_interop::fromCV3x1(t) 71 | }; 72 | } 73 | 74 | // TODO: clean up how we load calibration data. Allow passing in a 75 | // precomputed undistortion map. 76 | bool LoadCameraParameters(const std::string& yaml, 77 | CameraParameters* params) { 78 | cv::FileStorage fs(yaml, cv::FileStorage::READ); 79 | if (!fs.isOpened()) { 80 | return false; 81 | } 82 | 83 | *params = {}; 84 | 85 | cv::Size image_size; 86 | cv::Mat intrinsics_matrix_gl; 87 | cv::Mat dist_coeffs; 88 | cv::Mat undistorted_intrinsics_matrix_gl; 89 | 90 | fs["imageSize"] >> image_size; 91 | fs["cameraMatrix_gl"] >> intrinsics_matrix_gl; 92 | fs["distCoeffs"] >> dist_coeffs; 93 | fs["newCameraMatrix_gl" ] >> undistorted_intrinsics_matrix_gl; 94 | 95 | params->resolution = { image_size.width, image_size.height }; 96 | 97 | params->intrinsics = cameraMatrixToIntrinsics(intrinsics_matrix_gl); 98 | for (int i = 0; i < 5; ++i) { 99 | params->dist_coeffs.push_back( 100 | static_cast(dist_coeffs.at(0, i))); 101 | } 102 | 103 | params->undistorted_intrinsics = cameraMatrixToIntrinsics( 104 | undistorted_intrinsics_matrix_gl); 105 | 106 | #if 0 107 | auto res = PortableFloatMapIO::read(undistort_map_gl_filename); 108 | if (!res.valid) { 109 | return false; 110 | } 111 | params->undistortion_map = res.rg; 112 | #endif 113 | 114 | return params; 115 | } 116 | 117 | bool LoadRGBDCameraParameters(const std::string& dir, 118 | RGBDCameraParameters* params) { 119 | cv::FileStorage fs( 120 | os::path::join(dir, "stereo_calibration.yaml"), 121 | cv::FileStorage::READ); 122 | if (!fs.isOpened()) { 123 | return false; 124 | } 125 | 126 | params->color = LoadCameraIntrinsics(fs, "color"); 127 | auto res = PortableFloatMapIO::read( 128 | os::path::join(dir, "color_undistort_map_gl.pfm2")); 129 | if (!res.valid) { 130 | return false; 131 | } 132 | params->color.undistortion_map = res.rg; 133 | 134 | params->depth = LoadCameraIntrinsics(fs, "depth"); 135 | res = PortableFloatMapIO::read( 136 | os::path::join(dir, "depth_undistort_map_gl.pfm2")); 137 | if (!res.valid) { 138 | return false; 139 | } 140 | params->depth.undistortion_map = res.rg; 141 | 142 | params->color_from_depth = LoadEuclideanTransform(fs, "colorFromDepth_gl"); 143 | params->depth_from_color = LoadEuclideanTransform(fs, "depthFromColor_gl"); 144 | 145 | // TODO: depth calibration should output a range instead of the hard coded 146 | // constants here. 147 | params->depth.depth_range = Range1f::fromMinMax(0.8f, 4.0f); 148 | params->color.depth_range = params->depth.depth_range; 149 | 150 | return true; 151 | } 152 | 153 | EuclideanTransform RGBDCameraParameters::ConvertToColorCameraFromWorld( 154 | const EuclideanTransform& depth_camera_from_world) const { 155 | return color_from_depth * depth_camera_from_world; 156 | } 157 | 158 | EuclideanTransform RGBDCameraParameters::ConvertToDepthCameraFromWorld( 159 | const EuclideanTransform& color_camera_from_world) const { 160 | return depth_from_color * color_camera_from_world; 161 | } 162 | -------------------------------------------------------------------------------- /src/rgbd_camera_parameters.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef RGBD_CAMERA_PARAMETERS_H 15 | #define RGBD_CAMERA_PARAMETERS_H 16 | 17 | #include "libcgt/core/cameras/Intrinsics.h" 18 | #include "libcgt/core/common/Array2D.h" 19 | #include "libcgt/core/vecmath/EuclideanTransform.h" 20 | #include "libcgt/core/vecmath/Range1f.h" 21 | #include "libcgt/core/vecmath/Vector2i.h" 22 | 23 | #include 24 | 25 | struct CameraParameters { 26 | using Intrinsics = libcgt::core::cameras::Intrinsics; 27 | 28 | Vector2i resolution; 29 | // GL-style intrinsics. y-up, half-integer pixel centers. 30 | Intrinsics intrinsics; 31 | std::vector dist_coeffs; 32 | Array2D undistortion_map; // y axis points up. 33 | 34 | Intrinsics undistorted_intrinsics; 35 | 36 | // TODO: move this into depth parameters only 37 | Range1f depth_range; // In meters. 38 | }; 39 | 40 | struct RGBDCameraParameters { 41 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 42 | 43 | CameraParameters color; 44 | CameraParameters depth; 45 | 46 | // Extrinsic calibration between the two cameras. 47 | EuclideanTransform depth_from_color; // In meters. 48 | EuclideanTransform color_from_depth; // In meters. 49 | 50 | // TODO: Add a noise model. 51 | 52 | // Using the known extrinsic calibration between the color and depth 53 | // cameras, convert a depth camera_from_world pose to a color 54 | // camera_from_world pose. 55 | EuclideanTransform ConvertToColorCameraFromWorld( 56 | const EuclideanTransform& depth_camera_from_world) const; 57 | 58 | // Using the known extrinsic calibration between the color and depth 59 | // cameras, convert a color camera_from_world pose to a depth 60 | // camera_from_world pose. 61 | EuclideanTransform ConvertToDepthCameraFromWorld( 62 | const EuclideanTransform& color_camera_from_world) const; 63 | }; 64 | 65 | // Load a RGBDCameraParameters from a .yaml file. 66 | // 67 | // On success, populates params and returns true. 68 | // Otherwise, returns false. 69 | bool LoadCameraParameters(const std::string& yaml, 70 | CameraParameters* params); 71 | 72 | // Load a RGBDCameraParameters from a directory containing: 73 | // /stereo_calibration.yaml, 74 | // /color_undistort_map_gl.pfm2 75 | // /depth_undistort_map_gl.pfm2 76 | // Calls LoadCameraIntrinsics() on the YAML file with "color" and 'depth" as 77 | // prefixes for the intrinsics. Also reads "colorFromDepth_gl" and 78 | // "depthFromColor_gl" for extrinsics. 79 | // 80 | // On success, populates params and returns true. 81 | // Otherwise, returns false. 82 | bool LoadRGBDCameraParameters(const std::string& dir, 83 | RGBDCameraParameters* params); 84 | 85 | #endif // RGBD_CAMERA_PARAMETERS_H 86 | -------------------------------------------------------------------------------- /src/rgbd_input.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "rgbd_input.h" 15 | 16 | #include 17 | 18 | #include "libcgt/camera_wrappers/Kinect1x/KinectUtils.h" 19 | #include "libcgt/camera_wrappers/PixelFormat.h" 20 | #include "libcgt/core/common/ArrayUtils.h" 21 | #include "libcgt/core/imageproc/ColorMap.h" 22 | #include "libcgt/core/imageproc/Swizzle.h" 23 | 24 | #include "input_buffer.h" 25 | 26 | using libcgt::camera_wrappers::PixelFormat; 27 | using libcgt::camera_wrappers::RGBDInputStream; 28 | using libcgt::camera_wrappers::kinect1x::rawDepthMapToMeters; 29 | using libcgt::core::arrayutils::copy; 30 | using libcgt::core::arrayutils::flipY; 31 | using libcgt::core::imageproc::linearRemapToLuminance; 32 | using libcgt::core::imageproc::RGBToBGR; 33 | 34 | RgbdInput::RgbdInput(InputType input_type, const char* filename) : 35 | input_type_(input_type) { 36 | if (input_type == InputType::OPENNI2) { 37 | std::vector config; 38 | config.emplace_back( 39 | StreamType::COLOR, 40 | Vector2i{ 640, 480 }, PixelFormat::RGB_U888, 30, 41 | false 42 | ); 43 | config.emplace_back( 44 | StreamType::DEPTH, 45 | Vector2i{ 640, 480 }, PixelFormat::DEPTH_MM_U16, 30, 46 | false 47 | ); 48 | openni2_camera_ = std::make_unique(config); 49 | openni2_buffer_rgb_.resize(openni2_camera_->colorConfig().resolution); 50 | openni2_buffer_depth_.resize(openni2_camera_->depthConfig().resolution); 51 | openni2_frame_.color = openni2_buffer_rgb_.writeView(); 52 | openni2_frame_.depth = openni2_buffer_depth_.writeView(); 53 | openni2_camera_->start(); 54 | } else if (input_type == InputType::FILE) { 55 | file_input_stream_ = std::make_unique(filename); 56 | 57 | // Find the rgb stream. 58 | // TODO: take a dependency on cpp11-range 59 | for(int i = 0; i < file_input_stream_->metadata().size(); ++i) { 60 | const auto& metadata = file_input_stream_->metadata()[i]; 61 | if(metadata.type == StreamType::COLOR && 62 | metadata.format == PixelFormat::RGB_U888) { 63 | color_stream_id_ = i; 64 | color_metadata_ = metadata; 65 | break; 66 | } 67 | } 68 | 69 | // Find the depth stream. 70 | // TODO: take a dependency on cpp11-range 71 | for (int i = 0; i < file_input_stream_->metadata().size(); ++i) { 72 | const auto& metadata = file_input_stream_->metadata()[i]; 73 | if (metadata.type == StreamType::DEPTH) { 74 | raw_depth_stream_id_ = i; 75 | depth_metadata_ = metadata; 76 | break; 77 | } 78 | } 79 | } 80 | } 81 | 82 | Vector2i RgbdInput::colorSize() const { 83 | return color_metadata_.size; 84 | } 85 | 86 | Vector2i RgbdInput::depthSize() const { 87 | return depth_metadata_.size; 88 | } 89 | 90 | // TODO: simplify: read only rgb, etc 91 | // TODO: by default, read all 92 | 93 | void RgbdInput::read(InputBuffer* buffer, 94 | bool* rgb_updated, bool* depth_updated) { 95 | assert(rgb_updated != nullptr); 96 | assert(depth_updated != nullptr); 97 | 98 | // TODO: warn if input buffer is the wrong size. Resize it? 99 | 100 | *rgb_updated = false; 101 | *depth_updated = false; 102 | 103 | if (input_type_ == InputType::OPENNI2) { 104 | // TODO: if closed, return false 105 | 106 | bool succeeded = openni2_camera_->pollOne(openni2_frame_); 107 | if (openni2_frame_.colorUpdated) { 108 | // Copy the buffer, flipping it upside down for OpenGL. 109 | copy(openni2_frame_.color, flipY(buffer->color_rgb.writeView())); 110 | // Convert RGB to BGR for OpenCV. 111 | RGBToBGR(openni2_frame_.color, buffer->color_bgr_ydown.writeView()); 112 | buffer->color_timestamp_ns = openni2_frame_.colorTimestampNS; 113 | buffer->color_frame_index = openni2_frame_.colorFrameNumber; 114 | *rgb_updated = openni2_frame_.colorUpdated; 115 | } 116 | 117 | if (openni2_frame_.depthUpdated) { 118 | rawDepthMapToMeters(openni2_frame_.depth, buffer->depth_meters, 119 | false, true); 120 | buffer->depth_timestamp_ns = openni2_frame_.depthTimestampNS; 121 | buffer->depth_frame_index = openni2_frame_.depthFrameNumber; 122 | *depth_updated = openni2_frame_.depthUpdated; 123 | } 124 | 125 | } else if (input_type_ == InputType::FILE) { 126 | uint32_t stream_id; 127 | int64_t timestamp_ns; 128 | int32_t frame_index; 129 | Array1DReadView src = file_input_stream_->read( 130 | stream_id, frame_index, timestamp_ns); 131 | 132 | if (src.notNull()) { 133 | if (stream_id == color_stream_id_) { 134 | Array2DReadView src_rgb( 135 | src.pointer(), color_metadata_.size); 136 | // Copy the buffer, flipping it upside down for OpenGL. 137 | copy(src_rgb, flipY(buffer->color_rgb.writeView())); 138 | // Convert RGB to BGR for OpenCV. 139 | RGBToBGR(src_rgb, buffer->color_bgr_ydown.writeView()); 140 | buffer->color_timestamp_ns = timestamp_ns; 141 | buffer->color_frame_index = frame_index; 142 | 143 | //RGBToBGR(src_rgb, buffer->color_bgr_ydown.writeView()); 144 | //bool succeeded = copy(src_rgb, flipY(buffer->color_rgb.writeView())); 145 | *rgb_updated = true; 146 | // *rgb_updated = succeeded; 147 | } else if (stream_id == raw_depth_stream_id_ ) { 148 | buffer->depth_timestamp_ns = timestamp_ns; 149 | buffer->depth_frame_index = frame_index; 150 | 151 | if (depth_metadata_.format == PixelFormat::DEPTH_MM_U16) { 152 | Array2DReadView src_depth(src.pointer(), 153 | depth_metadata_.size); 154 | rawDepthMapToMeters(src_depth, buffer->depth_meters, 155 | false); 156 | *depth_updated = true; 157 | } else if(depth_metadata_.format == PixelFormat::DEPTH_M_F32) { 158 | Array2DReadView src_depth(src.pointer(), 159 | depth_metadata_.size); 160 | bool succeeded = copy(src_depth, buffer->depth_meters.writeView()); 161 | *depth_updated = succeeded; 162 | } 163 | } 164 | } 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /src/rgbd_input.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef RGBD_INPUT_H 15 | #define RGBD_INPUT_H 16 | 17 | #include 18 | #include 19 | 20 | #include "libcgt/core/common/Array2D.h" 21 | #include "libcgt/core/common/BasicTypes.h" 22 | #include "libcgt/camera_wrappers/RGBDStream.h" 23 | #include "libcgt/camera_wrappers/OpenNI2/OpenNI2Camera.h" 24 | 25 | // TODO(jiawen): Figure out a way to forward declare RGBDInputStream. 26 | 27 | struct InputBuffer; 28 | 29 | // TODO(jiawen): When accepting a camera, take in: 30 | // - an array of StreamConfigs, which is generic. 31 | // - a calibration file, or empty string for "use factory calibration." 32 | class RgbdInput { 33 | public: 34 | 35 | enum class InputType { 36 | KINECT1X, 37 | OPENNI2, 38 | REALSENSE, 39 | FILE, 40 | }; 41 | 42 | RgbdInput() = default; 43 | RgbdInput(InputType input_type, const char* filename); 44 | 45 | Vector2i colorSize() const; 46 | Vector2i depthSize() const; 47 | 48 | // Read one frame. If rgb_updated is set to true, then buffer->color_rgb 49 | // will be updated. Likewise, if depth_updated is set to true, then 50 | // buffer->depth_meters will be updated. Both might be set to false, in which 51 | // case the read failed. 52 | // 53 | // TODO: return a status struct, indicating if end of file is reached. 54 | void read(InputBuffer* buffer, bool* rgb_updated, bool* depth_updated); 55 | 56 | private: 57 | 58 | using OpenNI2Camera = libcgt::camera_wrappers::openni2::OpenNI2Camera; 59 | using RGBDInputStream = libcgt::camera_wrappers::RGBDInputStream; 60 | using StreamMetadata = libcgt::camera_wrappers::StreamMetadata; 61 | 62 | InputType input_type_; 63 | 64 | std::unique_ptr openni2_camera_; 65 | Array2D openni2_buffer_rgb_; 66 | Array2D openni2_buffer_depth_; 67 | OpenNI2Camera::FrameView openni2_frame_; 68 | 69 | std::unique_ptr file_input_stream_; 70 | 71 | int color_stream_id_ = -1; 72 | StreamMetadata color_metadata_; 73 | 74 | int raw_depth_stream_id_ = -1; 75 | StreamMetadata depth_metadata_; 76 | }; 77 | 78 | #endif // RGBD_INPUT_H 79 | -------------------------------------------------------------------------------- /src/shaders/draw_color.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | in VertexData 18 | { 19 | layout(location = 0) in vec4 vColor; 20 | }; 21 | 22 | layout(location = 0) out vec4 color; 23 | 24 | void main() 25 | { 26 | color = vColor; 27 | } 28 | )" -------------------------------------------------------------------------------- /src/shaders/draw_color_discard_transparent.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | in VertexData 18 | { 19 | layout(location = 0) in vec4 vColor; 20 | }; 21 | 22 | layout(location = 0) out vec4 color; 23 | 24 | void main() 25 | { 26 | if (vColor.w < 1) 27 | { 28 | discard; 29 | } 30 | color = vColor; 31 | } 32 | )" -------------------------------------------------------------------------------- /src/shaders/draw_single_color.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform vec4 uColor; 18 | 19 | layout(location = 0) out vec4 color; 20 | 21 | void main() 22 | { 23 | color = uColor; 24 | } 25 | )" -------------------------------------------------------------------------------- /src/shaders/draw_texture.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform sampler2D uSampler; 18 | // Initialize to identity, scalar constructor is along *diagonal*. 19 | layout(location = 1) uniform mat4 uColorMatrix = mat4(1.0); 20 | 21 | in VertexData 22 | { 23 | layout(location = 0) in vec2 vTex; 24 | }; 25 | 26 | layout(location = 0) out vec4 outputColor; 27 | 28 | void main() 29 | { 30 | vec4 inputColor = texture(uSampler, vTex); 31 | outputColor = uColorMatrix * inputColor; 32 | } 33 | )" -------------------------------------------------------------------------------- /src/shaders/draw_world_normal.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | in VertexData 18 | { 19 | layout(location = 0) in vec4 vWorldPos; 20 | layout(location = 1) in vec3 vWorldNormal; 21 | }; 22 | 23 | layout(location = 0) out vec4 oColor; 24 | 25 | void main() 26 | { 27 | oColor.xyz = 0.5 * (normalize(vWorldNormal) + vec3(1.0)); 28 | oColor.w = 1.0; 29 | } 30 | )" -------------------------------------------------------------------------------- /src/shaders/position_color.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform mat4 uCameraFromWorld; 18 | 19 | layout(location = 0) in vec4 aPos; 20 | layout(location = 1) in vec4 aColor; 21 | 22 | out gl_PerVertex 23 | { 24 | vec4 gl_Position; 25 | }; 26 | 27 | out VertexData 28 | { 29 | layout(location = 0) vec4 vColor; 30 | }; 31 | 32 | void main() 33 | { 34 | gl_Position = uCameraFromWorld * aPos; 35 | vColor = aColor; 36 | } 37 | )" -------------------------------------------------------------------------------- /src/shaders/position_normal.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform mat4 uClipFromWorld; 18 | // Initialize to identity, scalar constructor is along *diagonal*. 19 | layout(location = 1) uniform mat4 uWorldFromObject = mat4(1.0); 20 | layout(location = 2) uniform mat3 uWorldNormalFromObject = mat3(1.0); 21 | 22 | layout(location = 0) in vec4 aPos; 23 | layout(location = 1) in vec3 aNormal; 24 | 25 | out gl_PerVertex 26 | { 27 | vec4 gl_Position; 28 | }; 29 | 30 | out VertexData 31 | { 32 | layout(location = 0) out vec4 vWorldPos; 33 | layout(location = 1) out vec3 vWorldNormal; 34 | }; 35 | 36 | void main() 37 | { 38 | vWorldPos = uWorldFromObject * aPos; 39 | //vWorldNormal = uWorldNormalFromObject * aNormal; 40 | vWorldNormal = aNormal; 41 | 42 | gl_Position = uClipFromWorld * vWorldPos; 43 | } 44 | )" -------------------------------------------------------------------------------- /src/shaders/position_only.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform mat4 uCameraFromWorld; 18 | // Initialize to identity, scalar constructor is along *diagonal*. 19 | layout(location = 1) uniform mat4 uWorldFromObject = mat4(1.0); 20 | 21 | layout(location = 0) in vec4 aPos; 22 | 23 | out gl_PerVertex 24 | { 25 | vec4 gl_Position; 26 | }; 27 | 28 | void main() 29 | { 30 | gl_Position = uCameraFromWorld * uWorldFromObject * aPos; 31 | } 32 | )" -------------------------------------------------------------------------------- /src/shaders/position_texcoord.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform mat4 uCameraFromWorld; 18 | // Initialize to identity, scalar constructor is along *diagonal*. 19 | layout(location = 1) uniform mat4 uWorldFromObject = mat4(1.0); 20 | 21 | layout(location = 0) in vec4 aPos; 22 | layout(location = 1) in vec2 aTex; 23 | 24 | out gl_PerVertex 25 | { 26 | vec4 gl_Position; 27 | }; 28 | 29 | out VertexData 30 | { 31 | layout(location = 0) out vec2 vTex; 32 | }; 33 | 34 | void main() 35 | { 36 | gl_Position = uCameraFromWorld * uWorldFromObject * aPos; 37 | vTex = aTex; 38 | } 39 | )" -------------------------------------------------------------------------------- /src/shaders/unproject_point_cloud.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | layout(location = 0) uniform mat4 uClipFromWorld; // Viewing camera. 18 | 19 | // Depth camera intrinsics. 20 | layout(location = 1) uniform vec4 uDepthCameraFLPP; 21 | layout(location = 2) uniform vec2 uDepthCameraRangeMinMax; 22 | // Depth camera extrinsics. 23 | layout(location = 3) uniform mat4 uDepthWorldFromCamera; 24 | 25 | // Depth map texture. 26 | layout(location = 4) uniform sampler2D uDepthSampler; 27 | 28 | // Color of the point. 29 | layout(location = 5) uniform vec4 uPointColor = vec4(1.0); 30 | 31 | // Incoming position is a point. 32 | layout(location = 0) in vec2 aPos; 33 | 34 | out gl_PerVertex 35 | { 36 | vec4 gl_Position; 37 | }; 38 | 39 | out VertexData 40 | { 41 | layout(location = 0) out vec4 vColor; 42 | }; 43 | 44 | void main() 45 | { 46 | // Unproject using focal length and principal point. 47 | // x = fx * X / Z + px 48 | // --> X = Z * (x - px) / fx. 49 | 50 | // Get the texture coordinates for this point. 51 | vec2 st = aPos / textureSize(uDepthSampler, 0); 52 | // Sample the depth texture to get a positive z. 53 | float z = texture2D(uDepthSampler, st).x; 54 | 55 | // If the sample is missing, output alpha = 0. 56 | // The fragment shader will discard it. 57 | float within_depth_range = 1; 58 | if (z < uDepthCameraRangeMinMax.x || z > uDepthCameraRangeMinMax.y) 59 | { 60 | within_depth_range = 0; 61 | } 62 | 63 | vec2 fl = uDepthCameraFLPP.xy; 64 | vec2 pp = uDepthCameraFLPP.zw; 65 | vec2 xy = z * (aPos - pp) / fl; 66 | 67 | // Now put it in depth camera coordinates. 68 | // Flip z coordinate for OpenGL and set w = 1. 69 | vec4 xyzw_depth_camera = vec4(xy, -z, 1); 70 | 71 | // Now transform it into world coordinates. 72 | vec4 xyzw_world = uDepthWorldFromCamera * xyzw_depth_camera; 73 | 74 | gl_Position = uClipFromWorld * xyzw_world; 75 | vColor = within_depth_range * uPointColor; 76 | } 77 | )" -------------------------------------------------------------------------------- /src/shaders/unproject_point_cloud_textured.vs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | // TODO(jiawen): rename these. 18 | layout(location = 0) uniform mat4 uCameraFromWorld; 19 | layout(location = 1) uniform vec4 uDepthCameraFLPP; 20 | layout(location = 2) uniform vec2 uDepthCameraRangeMinMax; 21 | layout(location = 3) uniform mat4 uDepthWorldFromCamera; 22 | layout(location = 4) uniform mat4 uColorClipFromWorld; 23 | 24 | layout(location = 5) uniform sampler2D uDepthSampler; 25 | layout(location = 6) uniform sampler2D uColorSampler; 26 | 27 | layout(location = 0) in vec2 aPos; 28 | 29 | out gl_PerVertex 30 | { 31 | vec4 gl_Position; 32 | }; 33 | 34 | out VertexData 35 | { 36 | layout(location = 0) out vec4 vColor; 37 | }; 38 | 39 | void main() 40 | { 41 | // Unproject using focal length and principal point. 42 | // x = f * X / Z + cx 43 | // --> X = Z * (x - cx) / f. 44 | 45 | // Get the texture coordinates for this point. 46 | vec2 st = aPos / textureSize(uDepthSampler, 0); 47 | // Sample the depth texture to get a positive z. 48 | float z = texture2D(uDepthSampler, st).x; 49 | 50 | // If the sample is missing, output alpha = 0. 51 | // The fragment shader will discard it. 52 | float within_depth_range = 1; 53 | if (z < uDepthCameraRangeMinMax.x || z > uDepthCameraRangeMinMax.y) 54 | { 55 | within_depth_range = 0; 56 | } 57 | 58 | vec2 fl = uDepthCameraFLPP.xy; 59 | vec2 pp = uDepthCameraFLPP.zw; 60 | vec2 xy = z * (aPos - pp) / fl; 61 | 62 | // Now put it in depth camera coordinates. 63 | // Flip z coordinate for OpenGL and set w = 1. 64 | vec4 xyzw_depth_camera = vec4(xy, -z, 1); 65 | 66 | // Now transform it into world coordinates. 67 | vec4 xyzw_world = uDepthWorldFromCamera * xyzw_depth_camera; 68 | 69 | // Transform the point into color camera coordinates. 70 | vec4 xyzw_color_clip = uColorClipFromWorld * xyzw_world; 71 | vec2 st_color = xyzw_color_clip.xy / xyzw_color_clip.w; 72 | st_color = 0.5 * (st_color + vec2(1)); 73 | 74 | vec4 output_color; 75 | if (st_color.x >= 0 && st_color.y >= 0 && 76 | st_color.x <= 1 && st_color.y <= 1 && 77 | xyzw_color_clip.w > 0) 78 | { 79 | // TODO(jiawen): use texture2DProj. 80 | // TODO(jiawen): use sampler objects correctly. 81 | output_color = within_depth_range * texture2D(uColorSampler, st_color); 82 | } 83 | else 84 | { 85 | // If the point is visible but has no corresponding color pixel, 86 | // color it red. 87 | output_color = within_depth_range * vec4(1, 0, 0, 1); 88 | } 89 | 90 | gl_Position = uCameraFromWorld * xyzw_world; 91 | vColor = output_color; 92 | } 93 | )" -------------------------------------------------------------------------------- /src/shaders/visualize_texcoords.fs.glsl: -------------------------------------------------------------------------------- 1 | R"( 2 | // Copyright 2016 Google Inc. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http ://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | #version 450 16 | 17 | in VertexData 18 | { 19 | layout(location = 0) in vec2 vTex; 20 | }; 21 | 22 | layout(location = 0) out vec4 outputColor; 23 | 24 | void main() 25 | { 26 | outputColor = vec4(vTex, 0.0, 1.0); 27 | } 28 | )" -------------------------------------------------------------------------------- /src/single_moving_camera_gl_state.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #pragma once 15 | // TODO: use include guard 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "libcgt/core/common/Array2D.h" 25 | 26 | #include "libcgt/cuda/DeviceArray2D.h" 27 | #include "libcgt/cuda/gl/Texture2D.h" 28 | 29 | #include "libcgt/GL/GLProgramPipeline.h" 30 | #include "libcgt/GL/GLSamplerObject.h" 31 | #include "libcgt/GL/GLSeparableProgram.h" 32 | #include "libcgt/GL/GL_45/GLTexture2D.h" 33 | #include "libcgt/GL/GL_45/drawables/Axes.h" 34 | #include "libcgt/GL/GL_45/drawables/Frustum.h" 35 | #include "libcgt/GL/GL_45/drawables/PointCloud.h" 36 | #include "libcgt/GL/GL_45/drawables/TexturedRectangle.h" 37 | #include "libcgt/GL/GL_45/drawables/WireframeBox.h" 38 | 39 | #include "pipeline_data_type.h" 40 | #include "aruco/cube_fiducial.h" 41 | 42 | class RegularGridFusionPipeline; 43 | 44 | // TODO: rename this struct 45 | struct RemappedTexture { 46 | GLTexture2D* texture; 47 | Vector2f size_scale; 48 | Matrix4f color_transform; 49 | }; 50 | 51 | class SingleMovingCameraGLState : public QObject { 52 | 53 | Q_OBJECT 54 | 55 | public: 56 | SingleMovingCameraGLState(RegularGridFusionPipeline* pipeline, 57 | QOpenGLWidget* parent); 58 | 59 | void Resize(const Vector2i& size); 60 | void Render(const PerspectiveCamera& free_camera); 61 | 62 | public slots: 63 | 64 | void OnPipelineDataChanged(PipelineDataType type); 65 | 66 | private: 67 | 68 | // ----- Helper functions ----- 69 | void LoadShaders(); 70 | 71 | void DrawWorldAxes(); 72 | void DrawCubeFiducial(); 73 | void DrawUnprojectedPointCloud(); 74 | void DrawFullscreenRaycast(); 75 | void DrawCameraFrustaAndTSDFGrid(); 76 | void DrawInputsAndIntermediates(); 77 | void DrawRemappedTextures(const std::vector& textures); 78 | 79 | // ----- State ----- 80 | QOpenGLWidget* parent_ = nullptr; 81 | RegularGridFusionPipeline* pipeline_ = nullptr; 82 | PipelineDataType changed_pipeline_data_type_ = PipelineDataType::NONE; 83 | PerspectiveCamera free_camera_; 84 | 85 | // ----- Drawables ----- 86 | Axes world_axes_; 87 | TexturedRectangle input_buffer_textured_rect_; 88 | Frustum tracked_rgb_camera_; 89 | Frustum tracked_depth_camera_; 90 | WireframeBox tsdf_bbox_; 91 | PointCloud xy_coords_; 92 | CubeFiducialDrawable cube_fiducial_; 93 | 94 | GLTexture2D color_texture_; 95 | GLTexture2D color_tracking_vis_texture_; 96 | 97 | // TODO: can interop depth_texture. Then, can use undistorted version. 98 | GLTexture2D depth_texture_; // Float depth in meters. 99 | libcgt::cuda::gl::Texture2D smoothed_depth_tex_; 100 | libcgt::cuda::gl::Texture2D smoothed_incoming_normals_tex_; 101 | libcgt::cuda::gl::Texture2D pose_estimation_vis_tex_; 102 | 103 | // Raycasting from the current viewpoint. 104 | libcgt::cuda::gl::Texture2D raycasted_normals_tex_; 105 | 106 | // Raycasting from the current camera pose, resized as the view changes. 107 | DeviceArray2D free_camera_world_positions_; 108 | DeviceArray2D free_camera_world_normals_; 109 | libcgt::cuda::gl::Texture2D free_camera_world_positions_tex_; 110 | libcgt::cuda::gl::Texture2D free_camera_world_normals_tex_; 111 | 112 | // ----- GL helper objects ----- 113 | GLProgramPipeline draw_color_; 114 | GLProgramPipeline draw_single_color_; 115 | GLProgramPipeline draw_texture_; 116 | GLProgramPipeline unproject_point_cloud_; 117 | GLProgramPipeline visualize_texcoords_; 118 | 119 | GLSamplerObject nearest_sampler_; 120 | GLSamplerObject linear_sampler_; 121 | }; 122 | -------------------------------------------------------------------------------- /src/tsdf.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef TSDF_H 15 | #define TSDF_H 16 | 17 | #include 18 | 19 | class TSDF { 20 | public: 21 | 22 | ushort2 encoded_ = ushort2{ 0, 0 }; 23 | 24 | __inline__ __device__ __host__ 25 | TSDF() = default; 26 | 27 | __inline__ __device__ __host__ 28 | TSDF(float d, float w, float max_tsdf_value); 29 | 30 | __inline__ __device__ __host__ 31 | TSDF(const TSDF& copy) = default; 32 | 33 | __inline__ __device__ __host__ 34 | TSDF& operator = (const TSDF& copy) = default; 35 | 36 | __inline__ __device__ __host__ 37 | float2 Get(float max_tsdf_value) const; 38 | 39 | __inline__ __device__ __host__ 40 | float Distance(float max_tsdf_value) const; 41 | 42 | __inline__ __device__ __host__ 43 | float Weight() const; 44 | 45 | __inline__ __device__ __host__ 46 | void Set(float d, float w, float max_tsdf_value); 47 | 48 | __inline__ __device__ __host__ 49 | void Update(float incoming_d, float incoming_w, float max_tsdf_value); 50 | }; 51 | 52 | __inline__ __device__ __host__ 53 | TSDF::TSDF(float d, float w, float max_tsdf_value) { 54 | Set(d, w, max_tsdf_value); 55 | } 56 | 57 | __inline__ __device__ __host__ 58 | float2 TSDF::Get(float max_tsdf_value) const { 59 | return{ Distance(max_tsdf_value), Weight() }; 60 | } 61 | 62 | __inline__ __device__ __host__ 63 | float TSDF::Distance(float max_tsdf_value) const { 64 | return (2 * max_tsdf_value * (encoded_.x / 65535.f)) - max_tsdf_value; 65 | } 66 | 67 | __inline__ __device__ __host__ 68 | float TSDF::Weight() const { 69 | return static_cast(encoded_.y); 70 | } 71 | 72 | __inline__ __device__ __host__ 73 | void TSDF::Set(float d, float w, float max_tsdf_value) { 74 | encoded_ = { 75 | static_cast((d + max_tsdf_value) / (2 * max_tsdf_value) * 65535), 76 | static_cast(w) 77 | }; 78 | } 79 | 80 | __inline__ __device__ __host__ 81 | void TSDF::Update(float incoming_d, float incoming_w, float max_tsdf_value) { 82 | float2 old_tsdf = Get(max_tsdf_value); 83 | float old_d = old_tsdf.x; 84 | float old_w = old_tsdf.y; 85 | 86 | float new_w = old_w + incoming_w; 87 | float new_d = (old_w * old_d + incoming_w * incoming_d) / new_w; 88 | 89 | Set(new_d, new_w, max_tsdf_value); 90 | } 91 | 92 | #endif // TSDF_H 93 | -------------------------------------------------------------------------------- /src/visualize_camera_path/main_widget.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "main_widget.h" 15 | 16 | #include 17 | 18 | #include "libcgt/core/common/ArrayView.h" 19 | #include "libcgt/core/common/ArrayUtils.h" 20 | #include "libcgt/core/geometry/RectangleUtils.h" 21 | #include "libcgt/core/imageproc/ColorMap.h" 22 | #include "libcgt/core/math/MathUtils.h" 23 | #include "libcgt/GL/GLSeparableProgram.h" 24 | #include "libcgt/GL/GLUtilities.h" 25 | 26 | const float kFreeCameraFovYRadians = 0.87f; // 50 degrees. 27 | const float kFreeCameraZNear = 0.001f; 28 | const float kFreeCameraZFar = 100.0f; 29 | 30 | using libcgt::core::arrayutils::cast; 31 | using libcgt::core::cameras::GLFrustum; 32 | using libcgt::core::cameras::Intrinsics; 33 | using libcgt::core::imageproc::linearRemapToLuminance; 34 | using libcgt::core::math::clamp; 35 | using libcgt::core::vecmath::EuclideanTransform; 36 | using libcgt::core::vecmath::inverse; 37 | using libcgt::core::geometry::translate; 38 | 39 | namespace { 40 | const std::string kPositionColorVSSrc = 41 | #include "../shaders/position_color.vs.glsl" 42 | ; 43 | const std::string kPositionOnlyVSSrc = 44 | #include "../shaders/position_only.vs.glsl" 45 | ; 46 | const std::string kDrawColorFSSrc = 47 | #include "../shaders/draw_color.fs.glsl" 48 | ; 49 | const std::string kDrawSingleColorFSSrc = 50 | #include "../shaders/draw_single_color.fs.glsl" 51 | ; 52 | } // namespace 53 | 54 | namespace { 55 | void DebugCallback(GLenum source, GLenum type, GLuint id, GLenum severity, 56 | GLsizei length, const GLchar* message, const void* userParam) { 57 | if (severity != GL_DEBUG_SEVERITY_NOTIFICATION) { 58 | fprintf(stderr, "[GL DEBUG]: %s\n", message); 59 | } 60 | } 61 | } 62 | 63 | MainWidget::MainWidget(QWidget* parent) : 64 | QOpenGLWidget(parent) { 65 | QSurfaceFormat format = QSurfaceFormat::defaultFormat(); 66 | format.setProfile(QSurfaceFormat::CoreProfile); 67 | format.setMajorVersion(4); 68 | format.setMinorVersion(5); 69 | #if _DEBUG 70 | format.setOption(QSurfaceFormat::DebugContext, true); 71 | #endif 72 | setFormat(format); 73 | 74 | const float aspectRatio = static_cast(width()) / height(); 75 | free_camera_.setFrustum(GLFrustum::makeSymmetricPerspective( 76 | kFreeCameraFovYRadians, aspectRatio, kFreeCameraZNear, kFreeCameraZFar)); 77 | } 78 | 79 | void MainWidget::addCameraPath(const std::vector& poses, 80 | const CameraParameters& parameters, 81 | const Vector4f& color0, const Vector4f& color1) { 82 | makeCurrent(); 83 | 84 | FrustumPath path(poses.size()); 85 | for (int i = 0; i < poses.size(); ++i) 86 | { 87 | PerspectiveCamera camera; 88 | // TODO: replace hacky depth range with constants. 89 | camera.setFrustum(parameters.intrinsics, Vector2f(parameters.resolution), 90 | 0.02f, 0.1f ); 91 | camera.setCameraFromWorld( poses[ i ] ); 92 | 93 | float frac = static_cast< float >( i ) / ( poses.size() ); 94 | path[i].updateColor( 95 | libcgt::core::math::lerp(color0, color1, frac)); 96 | path[i].updatePositions( 97 | camera); 98 | } 99 | 100 | paths_.push_back(std::move(path)); 101 | sampling_rate_.push_back(1); 102 | 103 | if( selected_path_ == -1 ) { 104 | selected_path_ = 0; 105 | } 106 | 107 | doneCurrent(); 108 | } 109 | 110 | void MainWidget::initializeGL() { 111 | GLenum err = glewInit(); 112 | if (err != GLEW_OK) { 113 | fprintf(stderr, "Error initializing GLEW: %s\n", glewGetErrorString(err)); 114 | exit(1); 115 | } 116 | 117 | #if _DEBUG 118 | glEnable(GL_DEBUG_OUTPUT_SYNCHRONOUS); 119 | glDebugMessageCallback(DebugCallback, this); 120 | #endif 121 | 122 | glClearColor(0, 0, 0, 0); 123 | glEnable(GL_DEPTH_TEST); 124 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 125 | glBlendEquation(GL_FUNC_ADD); 126 | 127 | // Load shaders. 128 | auto positionColorVS = std::make_shared( 129 | GLSeparableProgram::Type::VERTEX_SHADER, kPositionColorVSSrc.c_str()); 130 | auto positionOnlyVS = std::make_shared( 131 | GLSeparableProgram::Type::VERTEX_SHADER, kPositionOnlyVSSrc.c_str()); 132 | auto drawColorFS = std::make_shared( 133 | GLSeparableProgram::Type::FRAGMENT_SHADER, kDrawColorFSSrc.c_str()); 134 | auto drawSingleColorFS = std::make_shared( 135 | GLSeparableProgram::Type::FRAGMENT_SHADER, kDrawSingleColorFSSrc.c_str()); 136 | 137 | draw_color_ = std::make_unique(); 138 | draw_color_->attachProgram(positionColorVS); 139 | draw_color_->attachProgram(drawColorFS); 140 | 141 | draw_single_color_ = std::make_unique(); 142 | draw_single_color_->attachProgram(positionOnlyVS); 143 | draw_single_color_->attachProgram(drawSingleColorFS); 144 | 145 | world_axes_ = std::make_unique(); 146 | } 147 | 148 | // virtual 149 | void MainWidget::keyPressEvent(QKeyEvent* e) { 150 | switch (e->key()) { 151 | case Qt::Key_Up: 152 | if (selected_path_ != -1) { 153 | int i = selected_path_; 154 | int s = clamp(sampling_rate_[i] + 1, Range1i(1, paths_[i].size() / 3)); 155 | sampling_rate_[i] = s; 156 | printf("path %d sampling rate is now: %d\n", i, s); 157 | update(); 158 | } 159 | break; 160 | case Qt::Key_Down: 161 | if (selected_path_ != -1) { 162 | int i = selected_path_; 163 | int s = clamp(sampling_rate_[i] - 1, Range1i(1, paths_[i].size() / 3)); 164 | sampling_rate_[i] = s; 165 | printf("path %d sampling rate is now: %d\n", i, s); 166 | } 167 | break; 168 | case Qt::Key_Left: 169 | selected_path_ = clamp(selected_path_ - 1, Range1i(paths_.size())); 170 | printf("selected path: %d\n", selected_path_); 171 | break; 172 | case Qt::Key_Right: 173 | selected_path_ = clamp(selected_path_ + 1, Range1i(paths_.size())); 174 | printf("selected path: %d\n", selected_path_); 175 | break; 176 | } 177 | } 178 | 179 | void MainWidget::paintGL() { 180 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 181 | 182 | // Draw world axes. 183 | draw_color_->vertexProgram()->setUniformMatrix4f(0, 184 | free_camera_.viewProjectionMatrix()); 185 | draw_color_->bind(); 186 | world_axes_->draw(); 187 | 188 | // Draw paths. 189 | for (size_t i = 0; i < paths_.size(); ++i) { 190 | auto& path = paths_[i]; 191 | int sampling_rate = sampling_rate_[i]; 192 | for (size_t j = 0; j < path.size(); j += sampling_rate) { 193 | path[j].draw(); 194 | } 195 | } 196 | 197 | GLProgramPipeline::unbindAll(); 198 | } 199 | 200 | void MainWidget::resizeGL(int w, int h) { 201 | glViewport(0, 0, w, h); 202 | 203 | float aspectRatio = static_cast(w) / h; 204 | free_camera_.setFrustum(GLFrustum::makeSymmetricPerspective( 205 | kFreeCameraFovYRadians, aspectRatio, kFreeCameraZNear, kFreeCameraZFar)); 206 | 207 | update(); 208 | } 209 | 210 | void MainWidget::mousePressEvent(QMouseEvent* e) { 211 | fps_controls_.handleMousePressEvent(e); 212 | update(); 213 | } 214 | 215 | void MainWidget::mouseMoveEvent(QMouseEvent* e) { 216 | fps_controls_.handleMouseMoveEvent(e, free_camera_); 217 | update(); 218 | } 219 | 220 | void MainWidget::mouseReleaseEvent(QMouseEvent* e) { 221 | fps_controls_.handleMouseReleaseEvent(e); 222 | update(); 223 | } 224 | -------------------------------------------------------------------------------- /src/visualize_camera_path/main_widget.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef MAIN_WIDGET_H 15 | #define MAIN_WIDGET_H 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "libcgt/core/cameras/PerspectiveCamera.h" 24 | #include "libcgt/core/common/Array2D.h" 25 | #include "libcgt/core/common/ArrayUtils.h" 26 | #include "libcgt/core/common/BasicTypes.h" 27 | #include "libcgt/core/vecmath/EuclideanTransform.h" 28 | #include "libcgt/GL/GLProgramPipeline.h" 29 | #include "libcgt/GL/GL_45/drawables/Axes.h" 30 | #include "libcgt/GL/GL_45/drawables/Frustum.h" 31 | #include "libcgt/qt_interop/FPSControls.h" 32 | 33 | #include "../rgbd_camera_parameters.h" 34 | 35 | class MainWidget : public QOpenGLWidget 36 | { 37 | public: 38 | 39 | typedef std::vector FrustumPath; 40 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 41 | 42 | MainWidget(QWidget* parent = nullptr); 43 | 44 | void addCameraPath(const std::vector& poses, 45 | const CameraParameters& intrinsics, 46 | const Vector4f& color0, const Vector4f& color1); 47 | 48 | protected: 49 | virtual void initializeGL(); 50 | virtual void paintGL(); 51 | virtual void resizeGL(int w, int h); 52 | 53 | virtual void mousePressEvent(QMouseEvent* e) override; 54 | virtual void mouseMoveEvent(QMouseEvent* e) override; 55 | virtual void mouseReleaseEvent(QMouseEvent* e) override; 56 | 57 | virtual void keyPressEvent(QKeyEvent* e) override; 58 | 59 | private: 60 | 61 | int selected_path_ = -1; 62 | std::vector paths_; 63 | std::vector sampling_rate_; 64 | 65 | FPSControls fps_controls_; 66 | PerspectiveCamera free_camera_; 67 | 68 | std::unique_ptr draw_color_; 69 | std::unique_ptr draw_single_color_; 70 | 71 | std::unique_ptr world_axes_; 72 | }; 73 | 74 | #endif // MAIN_WIDGET_H 75 | -------------------------------------------------------------------------------- /src/visualize_camera_path/visualize_camera_path_cli.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "libcgt/camera_wrappers/PoseStream.h" 6 | 7 | #include "main_widget.h" 8 | #include "../rgbd_camera_parameters.h" 9 | 10 | using libcgt::core::vecmath::EuclideanTransform; 11 | using libcgt::core::vecmath::lerp; 12 | 13 | struct PoseFrame { 14 | int32_t frameIndex; 15 | int64_t timestamp; 16 | EuclideanTransform e; 17 | }; 18 | 19 | std::vector LoadPoses(const std::string& filename) { 20 | libcgt::camera_wrappers::PoseInputStream inputStream(filename.c_str()); 21 | auto metadata = inputStream.metadata(); 22 | std::vector poses; 23 | PoseFrame f; 24 | bool ok = inputStream.read(f.frameIndex, f.timestamp, 25 | f.e.rotation, f.e.translation); 26 | while(ok) 27 | { 28 | poses.push_back(f); 29 | ok = inputStream.read(f.frameIndex, f.timestamp, 30 | f.e.rotation, f.e.translation); 31 | } 32 | return poses; 33 | } 34 | 35 | int main(int argc, char* argv[]) { 36 | QApplication app(argc, argv); 37 | 38 | MainWidget main_widget; 39 | main_widget.show(); 40 | 41 | // TODO: make this a flag. 42 | std::string FLAGS_calibration_dir = "D:/tmp/slf/asus_calib_00000"; 43 | RGBDCameraParameters camera_params; 44 | bool ok = LoadRGBDCameraParameters(FLAGS_calibration_dir, &camera_params); 45 | if (!ok) { 46 | fprintf(stderr, "Error loading RGBD camera parameters from %s.\n", 47 | FLAGS_calibration_dir.c_str()); 48 | return 1; 49 | } 50 | 51 | auto aruco_poses = LoadPoses("d:/tmp/slf/still_life_00002/aruco.pose"); 52 | auto sfm_aligned_poses = LoadPoses("d:/tmp/slf/still_life_00002/sfm.aligned.pose"); 53 | 54 | Vector4f red{ 1, 0, 0, 1 }; 55 | Vector4f green{ 0, 1, 0, 1 }; 56 | Vector4f blue{ 0, 0, 1, 1 }; 57 | Vector4f yellow{ 1, 1, 0, 1 }; 58 | //main_widget.addCameraPath(aruco_poses, rgbd_params.color, red, red); 59 | //main_widget.addCameraPath(sfm_aligned_poses, rgbd_params.color, blue, blue); 60 | 61 | std::vector aruco_transforms; 62 | aruco_transforms.push_back(aruco_poses[0].e); 63 | aruco_transforms.push_back(aruco_poses[aruco_poses.size() - 1].e); 64 | 65 | std::vector lerp_path; 66 | int kNumSamples = 100; 67 | for (int i = 0; i < kNumSamples; ++i) { 68 | float t = static_cast(i) / (kNumSamples - 1); 69 | lerp_path.push_back(lerp(aruco_transforms[0], aruco_transforms[1], t)); 70 | } 71 | 72 | #if 0 73 | for (const auto& f : aruco_poses) { 74 | aruco_transforms.push_back 75 | } 76 | #endif 77 | 78 | main_widget.addCameraPath(aruco_transforms, camera_params.color, red, red); 79 | main_widget.addCameraPath(lerp_path, camera_params.color, blue, blue); 80 | 81 | return app.exec(); 82 | } 83 | -------------------------------------------------------------------------------- /src_deprecated/artoolkit_pose_estimator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "artoolkit_pose_estimator.h" 15 | 16 | #include 17 | 18 | #include 19 | 20 | #include "libcgt/core/vecmath/Matrix4f.h" 21 | 22 | #include "rgbd_camera_parameters.h" 23 | 24 | namespace { 25 | 26 | // Convert intrinsics from our internal format to ARParam. 27 | ARParam Convert(const CameraParameters& input) { 28 | ARParam output = {}; 29 | output.dist_function_version = 4; 30 | output.xsize = input.resolution.x; 31 | output.ysize = input.resolution.y; 32 | 33 | output.dist_factor[0] = static_cast(input.dist_coeffs[0]); // k1. 34 | output.dist_factor[1] = static_cast(input.dist_coeffs[1]); // k2. 35 | output.dist_factor[2] = static_cast(input.dist_coeffs[2]); // p1. 36 | output.dist_factor[3] = static_cast(input.dist_coeffs[3]); // p2. 37 | output.dist_factor[4] = 38 | static_cast(input.intrinsics.focalLength.x); 39 | output.dist_factor[5] = 40 | static_cast(input.intrinsics.focalLength.y); 41 | output.dist_factor[6] = 42 | static_cast(input.intrinsics.principalPoint.x); 43 | // y points up in CameraParameters. 44 | // TODO: might be able to get away with keep the image with y up. 45 | output.dist_factor[7] = 46 | static_cast(input.resolution.y) - 47 | static_cast(input.intrinsics.principalPoint.y); 48 | 49 | // ARToolkit has a notion of "scale factor", where the video may be captured 50 | // at a resolution different from that of the camera calibration. 51 | output.dist_factor[8] = static_cast(1.0); 52 | 53 | // ARToolkit keeps its matrices as mat[row][col]. 54 | for (int i = 0; i < 3; ++i) { 55 | for (int j = 0; j < 4; ++j) { 56 | output.mat[i][j] = static_cast(0.0); 57 | } 58 | } 59 | output.mat[2][2] = static_cast(1.0); 60 | 61 | output.mat[0][0] = static_cast(input.intrinsics.focalLength.x); 62 | output.mat[0][2] = static_cast(input.intrinsics.principalPoint.x); 63 | output.mat[1][1] = static_cast(input.intrinsics.focalLength.y); 64 | // Flip y since ARToolkit uses y-down. 65 | output.mat[1][2] = 66 | static_cast(input.resolution.y) - 67 | static_cast(input.intrinsics.principalPoint.y); 68 | 69 | return output; 70 | } 71 | 72 | // Convert the result of a pose estimate (from, e.g., 73 | // arGetTransMatMultiSquare(), into a Matrix4f representing a camera from world 74 | // transformation with y-up. 75 | Matrix4f ConvertToCameraFromWorldMeters(ARdouble matrix[3][4]) { 76 | double view_matrix[16]; 77 | arglCameraViewRH(matrix, view_matrix, 0.001); // scale factor: mm --> m. 78 | 79 | Matrix4f output; 80 | for (int k = 0; k < 16; ++k) { 81 | output[k] = static_cast(view_matrix[k]); 82 | } 83 | const Matrix4f rot90 = Matrix4f::rotateX(static_cast(M_PI_2)); 84 | return output * rot90; 85 | } 86 | 87 | } 88 | 89 | ARToolkitPoseEstimator::ARToolkitPoseEstimator( 90 | const CameraParameters& params, const std::string& marker_cube_filename) : 91 | luma_(params.resolution) { 92 | // Setup camera. 93 | ARParam camera_params = Convert(params); 94 | camera_params_lut_ = 95 | arParamLTCreate(&camera_params, AR_PARAM_LT_DEFAULT_OFFSET); 96 | tracker_ = arCreateHandle(camera_params_lut_); 97 | arSetPixelFormat(tracker_, pixel_format_); 98 | arSetMatrixCodeType(tracker_, AR_MATRIX_CODE_3x3_HAMMING63); 99 | 100 | #if DEBUG 101 | arSetDebugMode(tracker_, AR_DEBUG_ENABLE); 102 | #else 103 | arSetDebugMode(tracker_, AR_DEBUG_DISABLE); 104 | #endif 105 | tracker_3d_ = ar3DCreateHandle(&camera_params); 106 | 107 | // Setup cube marker. 108 | pattern_ = arPattCreateHandle(); 109 | 110 | // TODO: handle failure here. 111 | multi_marker_config_ = arMultiReadConfigFile(marker_cube_filename.c_str(), 112 | pattern_); 113 | // Yuck: ARToolkit doesn't set this rather important flag. 114 | multi_marker_config_->min_submarker = 1; 115 | 116 | // Set the detector type based on the marker type. 117 | const int patt_type = multi_marker_config_->patt_type; 118 | if (patt_type == AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE) { 119 | arSetPatternDetectionMode(tracker_, AR_TEMPLATE_MATCHING_COLOR); 120 | } else if (patt_type == AR_MULTI_PATTERN_DETECTION_MODE_MATRIX) { 121 | arSetPatternDetectionMode(tracker_, AR_MATRIX_CODE_DETECTION); 122 | } else { // AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE_AND_MATRIX 123 | arSetPatternDetectionMode(tracker_, AR_TEMPLATE_MATCHING_COLOR_AND_MATRIX); 124 | } 125 | 126 | // Attach pattern to tracker. 127 | int ret = arPattAttach(tracker_, pattern_); 128 | assert(ret == 0); 129 | } 130 | 131 | ARToolkitPoseEstimator::~ARToolkitPoseEstimator() { 132 | if (tracker_3d_ != nullptr) { 133 | ar3DDeleteHandle(&tracker_3d_); 134 | } 135 | if (tracker_ != nullptr) { 136 | arDeleteHandle(tracker_); 137 | } 138 | if (camera_params_lut_ != nullptr) { 139 | arParamLTFree(&camera_params_lut_); 140 | } 141 | } 142 | 143 | #include "libcgt/core/common/ArrayUtils.h" 144 | #include "libcgt/core/time/TimeUtils.h" 145 | 146 | ARToolkitPoseEstimator::Result ARToolkitPoseEstimator::EstimatePose( 147 | Array2DReadView bgr) { 148 | Result result; 149 | if (!bgr.packed()) { 150 | return result; 151 | } 152 | 153 | // Ugh need to convert anyway!. 154 | 155 | AR2VideoBufferT frame = {}; 156 | // TODO: move into libcgt::colormath, make map() work. 157 | // TODO: fixed point to luma 158 | libcgt::core::arrayutils::map(bgr, luma_.writeView(), 159 | [&] (uint8x3 bgr) 160 | { 161 | float z = 0.25f * bgr.x + 0.5f * bgr.y + 0.25f * bgr.z; 162 | return static_cast(z); 163 | } ); 164 | 165 | frame.buff = reinterpret_cast(const_cast(bgr.pointer())); 166 | frame.buffLuma = luma_.pointer(); 167 | frame.fillFlag = 1; 168 | // TODO: timestamps 169 | 170 | auto t0 = std::chrono::high_resolution_clock::now(); 171 | 172 | int detect_result = arDetectMarker(tracker_, &frame); 173 | if (detect_result == 0) { 174 | ARMarkerInfo* detected_markers = arGetMarker(tracker_); 175 | result.nMarkers = arGetMarkerNum(tracker_); 176 | if (result.nMarkers > 0) { 177 | // TODO: add a config option for this. 178 | //ARdouble err = arGetTransMatMultiSquare(tracker_3d_, detected_markers, 179 | ARdouble err = arGetTransMatMultiSquareRobust(tracker_3d_, detected_markers, 180 | result.nMarkers, multi_marker_config_); 181 | if (err != -1.0) { 182 | Matrix4f trans = ConvertToCameraFromWorldMeters( 183 | multi_marker_config_->trans); 184 | result.error = err; 185 | result.world_from_camera = inverse( 186 | EuclideanTransform::fromMatrix(trans)); 187 | result.valid = true; 188 | } 189 | } 190 | } 191 | 192 | auto t1 = std::chrono::high_resolution_clock::now(); 193 | if(result.valid) { 194 | printf("ARToolkit: succeeded, nMarkers = %d, error = %lf.\n", 195 | result.nMarkers, result.error); 196 | } else { 197 | printf("ARToolkit: failed.\n"); 198 | } 199 | printf("ARToolkit: pose estimation took %lld ms\n", 200 | libcgt::core::time::dtMS(t0, t1)); 201 | return result; 202 | } 203 | -------------------------------------------------------------------------------- /src_deprecated/artoolkit_pose_estimator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef AR_TOOLKIT_POSE_ESTIMATOR_H 15 | #define AR_TOOLKIT_POSE_ESTIMATOR_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/common/Array2D.h" 20 | #include "libcgt/core/common/BasicTypes.h" 21 | #include "libcgt/core/vecmath/EuclideanTransform.h" 22 | 23 | struct CameraParameters; 24 | 25 | class ARToolkitPoseEstimator { 26 | public: 27 | 28 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 29 | 30 | struct Result { 31 | bool valid = false; 32 | int nMarkers; 33 | double error; 34 | EuclideanTransform world_from_camera; 35 | }; 36 | 37 | ARToolkitPoseEstimator(const CameraParameters& params, 38 | const std::string& marker_cube_filename); 39 | ~ARToolkitPoseEstimator(); 40 | 41 | Result EstimatePose(Array2DReadView bgr); 42 | 43 | private: 44 | 45 | Array2D luma_; 46 | 47 | // TODO: AR_PIXEL_FORMAT_MONO is probably good enough! 48 | AR_PIXEL_FORMAT pixel_format_ = AR_PIXEL_FORMAT_BGR; 49 | ARParamLT* camera_params_lut_ = nullptr; 50 | ARHandle* tracker_ = nullptr; 51 | AR3DHandle* tracker_3d_ = nullptr; 52 | 53 | ARPattHandle* pattern_ = nullptr; 54 | ARMultiMarkerInfoT* multi_marker_config_ = nullptr; 55 | }; 56 | 57 | #endif // AR_TOOLKIT_POSE_ESTIMATOR_H 58 | -------------------------------------------------------------------------------- /src_deprecated/artoolkitplus_pose_estimator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include "artoolkitplus_pose_estimator.h" 15 | 16 | #include 17 | 18 | #include "libcgt/core/common/ArrayUtils.h" 19 | #include "libcgt/core/time/TimeUtils.h" 20 | #include "libcgt/core/vecmath/Matrix4f.h" 21 | 22 | #include "rgbd_camera_parameters.h" 23 | 24 | namespace { 25 | 26 | // Convert the result of a pose estimate (from, e.g., 27 | // arGetTransMatMultiSquare(), into a Matrix4f representing a camera from world 28 | // transformation with y-up. 29 | Matrix4f ConvertToCameraFromWorldMeters(const ARFloat matrix[3][4]) { 30 | ARFloat view_matrix[16]; 31 | view_matrix[0 + 0*4] = matrix[0][0]; // R1C1 32 | view_matrix[0 + 1*4] = matrix[0][1]; // R1C2 33 | view_matrix[0 + 2*4] = matrix[0][2]; 34 | view_matrix[0 + 3*4] = matrix[0][3]; 35 | view_matrix[1 + 0*4] = matrix[1][0]; // R2 36 | view_matrix[1 + 1*4] = matrix[1][1]; 37 | view_matrix[1 + 2*4] = matrix[1][2]; 38 | view_matrix[1 + 3*4] = matrix[1][3]; 39 | view_matrix[2 + 0*4] = matrix[2][0]; // R3 40 | view_matrix[2 + 1*4] = matrix[2][1]; 41 | view_matrix[2 + 2*4] = matrix[2][2]; 42 | view_matrix[2 + 3*4] = matrix[2][3]; 43 | view_matrix[3 + 0*4] = 0.0; 44 | view_matrix[3 + 1*4] = 0.0; 45 | view_matrix[3 + 2*4] = 0.0; 46 | view_matrix[3 + 3*4] = 1.0; 47 | 48 | Matrix4f output; 49 | for (int k = 0; k < 16; ++k) { 50 | output[k] = static_cast(view_matrix[k]); 51 | } 52 | // Convert from millimeters to meters. 53 | output.column3.xyz *= 0.001f; 54 | 55 | const Matrix4f rot90 = Matrix4f::rotateX(static_cast(M_PI_2)); 56 | return output * rot90; 57 | } 58 | 59 | } 60 | 61 | ARToolkitPlusPoseEstimator::ARToolkitPlusPoseEstimator( 62 | const CameraParameters& params, const std::string& marker_cube_filename) : 63 | rgbx_(params.resolution), 64 | tracker_( 65 | params.resolution.x, params.resolution.y, 66 | 6, // maxImagePatterns 67 | 6, 6 // pattern width and height 68 | ) { 69 | tracker_.initMarkerConfig(marker_cube_filename.c_str()); 70 | tracker_.initCamera(params.resolution.x, params.resolution.y, 71 | static_cast(params.intrinsics.focalLength.x), 72 | static_cast(params.intrinsics.focalLength.y), 73 | static_cast(params.intrinsics.principalPoint.x), 74 | // Flip y since ARToolkit uses y-down. 75 | static_cast( 76 | params.resolution.y - params.intrinsics.principalPoint.y), 77 | static_cast(params.dist_coeffs[0]), 78 | static_cast(params.dist_coeffs[1]), 79 | static_cast(params.dist_coeffs[2]), 80 | static_cast(params.dist_coeffs[3]), 81 | static_cast(params.dist_coeffs[4]), 82 | static_cast(params.depth_range.left()), 83 | static_cast(params.depth_range.right())); 84 | tracker_.setUseDetectLite(false); 85 | 86 | // TODO: flag 87 | tracker_.activateAutoThreshold(true); 88 | 89 | // TODO: necessary? 90 | tracker_.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGBA); 91 | } 92 | 93 | ARToolkitPlusPoseEstimator::Result ARToolkitPlusPoseEstimator::EstimatePose( 94 | Array2DReadView bgr) { 95 | Result result; 96 | if (!bgr.packed()) { 97 | return result; 98 | } 99 | 100 | // Ugh need to convert anyway!. 101 | auto t0 = std::chrono::high_resolution_clock::now(); 102 | 103 | libcgt::core::arrayutils::map(bgr, rgbx_.writeView(), 104 | [&] (uint8x3 bgr) 105 | { 106 | return uint8x4{bgr.z, bgr.y, bgr.x, 255}; 107 | }); 108 | 109 | tracker_.calc(reinterpret_cast(rgbx_.pointer())); 110 | 111 | result.nMarkers = tracker_.getNumDetectedMarkers(); 112 | if (result.nMarkers > 0) { 113 | Matrix4f camera_from_world = ConvertToCameraFromWorldMeters( 114 | tracker_.getMultiMarkerConfig()->trans); 115 | 116 | result.world_from_camera = inverse( 117 | EuclideanTransform::fromMatrix(camera_from_world)); 118 | result.valid = true; 119 | } 120 | 121 | auto t1 = std::chrono::high_resolution_clock::now(); 122 | if(result.valid) { 123 | printf("ARToolkitPlus: succeeded, nMarkers = %d, error = %lf.\n", 124 | result.nMarkers, result.error); 125 | } else { 126 | printf("ARToolkitPlus: failed.\n"); 127 | } 128 | printf( "ARToolkitPlus: pose estimation took %lld ms\n", 129 | libcgt::core::time::dtMS(t0, t1)); 130 | return result; 131 | } 132 | -------------------------------------------------------------------------------- /src_deprecated/artoolkitplus_pose_estimator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Google Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http ://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #ifndef AR_TOOLKITPLUS_POSE_ESTIMATOR_H 15 | #define AR_TOOLKITPLUS_POSE_ESTIMATOR_H 16 | 17 | #include 18 | 19 | #include "libcgt/core/common/Array2D.h" 20 | #include "libcgt/core/common/BasicTypes.h" 21 | #include "libcgt/core/vecmath/EuclideanTransform.h" 22 | 23 | struct CameraParameters; 24 | 25 | class ARToolkitPlusPoseEstimator { 26 | public: 27 | 28 | using EuclideanTransform = libcgt::core::vecmath::EuclideanTransform; 29 | 30 | struct Result { 31 | bool valid = false; 32 | int nMarkers; 33 | double error; 34 | EuclideanTransform world_from_camera; 35 | }; 36 | 37 | ARToolkitPlusPoseEstimator(const CameraParameters& params, 38 | const std::string& marker_cube_filename); 39 | 40 | Result EstimatePose(Array2DReadView bgr); 41 | 42 | private: 43 | 44 | Array2D rgbx_; 45 | 46 | // TODO: AR_PIXEL_FORMAT_MONO is probably good enough. 47 | ARToolKitPlus::PIXEL_FORMAT pixel_format_ = ARToolKitPlus::PIXEL_FORMAT_BGR; 48 | ARToolKitPlus::TrackerMultiMarker tracker_; 49 | }; 50 | 51 | #endif // AR_TOOLKITPLUS_POSE_ESTIMATOR_H 52 | -------------------------------------------------------------------------------- /third_party/bitmask_operators.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JSS_BITMASK_HPP 2 | #define JSS_BITMASK_HPP 3 | 4 | // (C) Copyright 2015 Just Software Solutions Ltd 5 | // 6 | // Distributed under the Boost Software License, Version 1.0. 7 | // 8 | // Boost Software License - Version 1.0 - August 17th, 2003 9 | // 10 | // Permission is hereby granted, free of charge, to any person or 11 | // organization obtaining a copy of the software and accompanying 12 | // documentation covered by this license (the "Software") to use, 13 | // reproduce, display, distribute, execute, and transmit the 14 | // Software, and to prepare derivative works of the Software, and 15 | // to permit third-parties to whom the Software is furnished to 16 | // do so, all subject to the following: 17 | // 18 | // The copyright notices in the Software and this entire 19 | // statement, including the above license grant, this restriction 20 | // and the following disclaimer, must be included in all copies 21 | // of the Software, in whole or in part, and all derivative works 22 | // of the Software, unless such copies or derivative works are 23 | // solely in the form of machine-executable object code generated 24 | // by a source language processor. 25 | // 26 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY 27 | // KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 28 | // WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 29 | // PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE 30 | // COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE 31 | // LIABLE FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN 32 | // CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 33 | // CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 34 | // THE SOFTWARE. 35 | 36 | #include 37 | 38 | template 39 | struct enable_bitmask_operators{ 40 | static const bool enable=false; 41 | }; 42 | 43 | template 44 | typename std::enable_if::enable,E>::type 45 | operator|(E lhs,E rhs){ 46 | typedef typename std::underlying_type::type underlying; 47 | return static_cast( 48 | static_cast(lhs) | static_cast(rhs)); 49 | } 50 | 51 | template 52 | typename std::enable_if::enable,E>::type 53 | operator&(E lhs,E rhs){ 54 | typedef typename std::underlying_type::type underlying; 55 | return static_cast( 56 | static_cast(lhs) & static_cast(rhs)); 57 | } 58 | 59 | template 60 | typename std::enable_if::enable,E>::type 61 | operator^(E lhs,E rhs){ 62 | typedef typename std::underlying_type::type underlying; 63 | return static_cast( 64 | static_cast(lhs) ^ static_cast(rhs)); 65 | } 66 | 67 | template 68 | typename std::enable_if::enable,E>::type 69 | operator~(E lhs){ 70 | typedef typename std::underlying_type::type underlying; 71 | return static_cast( 72 | ~static_cast(lhs)); 73 | } 74 | 75 | template 76 | typename std::enable_if::enable,E&>::type 77 | operator|=(E& lhs,E rhs){ 78 | typedef typename std::underlying_type::type underlying; 79 | lhs=static_cast( 80 | static_cast(lhs) | static_cast(rhs)); 81 | return lhs; 82 | } 83 | 84 | template 85 | typename std::enable_if::enable,E&>::type 86 | operator&=(E& lhs,E rhs){ 87 | typedef typename std::underlying_type::type underlying; 88 | lhs=static_cast( 89 | static_cast(lhs) & static_cast(rhs)); 90 | return lhs; 91 | } 92 | 93 | template 94 | typename std::enable_if::enable,E&>::type 95 | operator^=(E& lhs,E rhs){ 96 | typedef typename std::underlying_type::type underlying; 97 | lhs=static_cast( 98 | static_cast(lhs) ^ static_cast(rhs)); 99 | return lhs; 100 | } 101 | 102 | // TODO: why does the enable_if version not work? 103 | #if 1 104 | template 105 | bool isZero(E lhs){ 106 | typedef typename std::underlying_type::type underlying; 107 | return static_cast(lhs) == static_cast(0); 108 | } 109 | 110 | template 111 | bool notZero(E lhs) { 112 | typedef typename std::underlying_type::type underlying; 113 | return static_cast(lhs) != static_cast(0); 114 | } 115 | 116 | #else 117 | template 118 | bool isSet( 119 | typename std::enable_if::enable,E>::type value, 120 | typename std::enable_if::enable,E>::type query) { 121 | return notZero(E & query); 122 | } 123 | 124 | template 125 | bool isZero( 126 | typename std::enable_if::enable,E>::type lhs){ 127 | typedef typename std::underlying_type::type underlying; 128 | return static_cast(lhs) == static_cast(0); 129 | } 130 | 131 | template 132 | bool notZero( 133 | typename std::enable_if::enable,E>::type lhs) { 134 | typedef typename std::underlying_type::type underlying; 135 | return static_cast(lhs) != static_cast(0); 136 | } 137 | #endif 138 | 139 | #endif 140 | --------------------------------------------------------------------------------