diff --git a/docs/_static/Advanced/color_map_optimization/initial.png b/docs/_static/Advanced/color_map_optimization/initial.png new file mode 100644 index 00000000000..16385c71eea Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/initial.png differ diff --git a/docs/_static/Advanced/color_map_optimization/initial_zoom.png b/docs/_static/Advanced/color_map_optimization/initial_zoom.png new file mode 100644 index 00000000000..2572a850005 Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/initial_zoom.png differ diff --git a/docs/_static/Advanced/color_map_optimization/non_rigid.png b/docs/_static/Advanced/color_map_optimization/non_rigid.png new file mode 100644 index 00000000000..a17c10ece2b Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/non_rigid.png differ diff --git a/docs/_static/Advanced/color_map_optimization/non_rigid_zoom.png b/docs/_static/Advanced/color_map_optimization/non_rigid_zoom.png new file mode 100644 index 00000000000..34c42362628 Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/non_rigid_zoom.png differ diff --git a/docs/_static/Advanced/color_map_optimization/rigid.png b/docs/_static/Advanced/color_map_optimization/rigid.png new file mode 100644 index 00000000000..1f933317c8b Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/rigid.png differ diff --git a/docs/_static/Advanced/color_map_optimization/rigid_zoom.png b/docs/_static/Advanced/color_map_optimization/rigid_zoom.png new file mode 100644 index 00000000000..32bfac6a1de Binary files /dev/null and b/docs/_static/Advanced/color_map_optimization/rigid_zoom.png differ diff --git a/docs/tutorial/Advanced/color_map_optimization.rst b/docs/tutorial/Advanced/color_map_optimization.rst new file mode 100644 index 00000000000..d9f0fb225ef --- /dev/null +++ b/docs/tutorial/Advanced/color_map_optimization.rst @@ -0,0 +1,180 @@ +.. _color_map_optimization: + +Color Map Optimization +------------------------------------- + +Consider color mapping to the geometry reconstructed from depth cameras. As color and depth frames are not perfectly aligned, the texture mapping using color images is subject to results in blurred color map. Open3D provides color map optimization method proposed by [Zhou2014]_. Before begin, download fountain dataset from `here `_. The following script shows an example of color map optimization. + +.. code-block:: python + + from py3d import * + from trajectory_io import * + import os, sys + sys.path.append("../Utility") + from common import * + + path = "[set_this_path_to_fountain_dataset]" + debug_mode = False + + if __name__ == "__main__": + set_verbosity_level(VerbosityLevel.Debug) + + # Read RGBD images + rgbd_images = [] + depth_image_path = get_file_list( + os.path.join(path, "depth/"), extension = ".png") + color_image_path = get_file_list( + os.path.join(path, "image/"), extension = ".jpg") + assert(len(depth_image_path) == len(color_image_path)) + for i in range(len(depth_image_path)): + depth = read_image(os.path.join(depth_image_path[i])) + color = read_image(os.path.join(color_image_path[i])) + rgbd_image = create_rgbd_image_from_color_and_depth(color, depth, + convert_rgb_to_intensity = False) + if debug_mode: + pcd = create_point_cloud_from_rgbd_image(rgbd_image, + PinholeCameraIntrinsic.get_prime_sense_default()) + draw_geometries([pcd]) + rgbd_images.append(rgbd_image) + + # Read camera pose and mesh + camera = read_pinhole_camera_trajectory(os.path.join(path, "scene/key.log")) + mesh = read_triangle_mesh(os.path.join(path, "scene", "integrated.ply")) + + # Before full optimization, let's just visualize texture map + # with given geometry, RGBD images, and camera poses. + option = ColorMapOptmizationOption() + option.maximum_iteration = 0 + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_before_optimization.ply"), mesh) + + # Optimize texture and save the mesh as texture_mapped.ply + # This is implementation of following paper + # Q.-Y. Zhou and V. Koltun, + # Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras, + # SIGGRAPH 2014 + option.maximum_iteration = 500 + option.non_rigid_camera_coordinate = True + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_after_optimization.ply"), mesh) + +Input +```````````````````````` + +.. code-block:: python + + # read RGBD images + rgbd_images = [] + depth_image_path = get_file_list( + os.path.join(path, "depth/"), extension=".png") + color_image_path = get_file_list( + os.path.join(path, "image/"), extension=".jpg") + assert(len(depth_image_path) == len(color_image_path)) + for i in range(len(depth_image_path)): + depth = read_image(os.path.join(depth_image_path[i])) + color = read_image(os.path.join(color_image_path[i])) + rgbd_image = create_rgbd_image_from_color_and_depth(color, depth, + convert_rgb_to_intensity=False) + if debug_mode: + pcd = create_point_cloud_from_rgbd_image(rgbd_image, + PinholeCameraIntrinsic.get_prime_sense_default()) + draw_geometries([pcd]) + rgbd_images.append(rgbd_image) + +This script reads color and depth image pairs and makes ``rgbd_image``. Note that ``convert_rgb_to_intensity`` flag is ``False``. This is to preserve 8-bit color channels instead of using single channel float type image. + +It is always good practice to visualize RGBD image before applying it to color map optimization. ``debug_mode`` switch is for visualizing RGBD image. + +.. code-block:: python + + # read camera pose and mesh + camera = read_pinhole_camera_trajectory(os.path.join(path, "scene/key.log")) + mesh = read_triangle_mesh(os.path.join(path, "scene", "integrated.ply")) + +The script reads camera trajectory and mesh. + +.. code-block:: python + + option = ColorMapOptmizationOption() + option.maximum_iteration = 0 + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_before_optimization.ply"), mesh) + +To visualize how the camera poses are not good for color mapping, this script intentionally set the iteration number as 0, which means no optimization. ``color_map_optimization`` paints a mesh using corresponding RGBD images and camera poses. Without optimization, the texture map is blurred. + +.. image:: ../../_static/Advanced/color_map_optimization/initial.png + :width: 300px + +.. image:: ../../_static/Advanced/color_map_optimization/initial_zoom.png + :width: 300px + +Rigid Optimization +``````````````````````````````` + +The next step is to optimize camera poses to get a sharp color map. + +.. code-block:: python + + option.maximum_iteration = 500 + option.non_rigid_camera_coordinate = True + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_after_optimization.ply"), mesh) + +The script sets ``maximum_iteration = 500`` for actual iterations. The optimization displays the following energy profile. + +.. code-block:: shell + + [ColorMapOptimization] :: Rigid Optimization + [Iteration 0001] Residual error : 25777.372725 (avg : 0.004998) + [Iteration 0002] Residual error : 25620.681829 (avg : 0.004967) + [Iteration 0003] Residual error : 25463.806101 (avg : 0.004937) + : + [Iteration 0498] Residual error : 11550.014763 (avg : 0.002255) + [Iteration 0499] Residual error : 11549.850827 (avg : 0.002255) + [Iteration 0500] Residual error : 11550.062068 (avg : 0.002255) + +Residual error implies inconsistency of image intensities. Lower residual leads better color map quality. By default, ``ColorMapOptmizationOption`` enables rigid optimization. It optimizes 6-dimentional pose of every cameras. + +.. image:: ../../_static/Advanced/color_map_optimization/rigid.png + :width: 300px + +.. image:: ../../_static/Advanced/color_map_optimization/rigid_zoom.png + :width: 300px + +Non-rigid Optimization +``````````````````````````````````` + +For better alignment quality, there is an option for non-rigid optimization. To enable, simply add + +.. code-block:: python + + option.non_rigid_camera_coordinate = True + +before calling ``color_map_optimization``. Besides 6-dimentional camera poses, non-rigid optimization even consider local image warping represented by anchor points. This adds even more flexibility and leads higher quality color mapping. The residual error is smaller than the case of rigid optimization. + +.. code-block:: shell + + [ColorMapOptimization] :: Non-Rigid Optimization + [Iteration 0001] Residual error : 25777.372725, reg : 0.000000 + [Iteration 0002] Residual error : 25330.445704, reg : 13.005639 + [Iteration 0003] Residual error : 24885.912182, reg : 40.000765 + : + [Iteration 0498] Residual error : 7585.606850, reg : 3294.124184 + [Iteration 0499] Residual error : 7585.274846, reg : 3294.887659 + [Iteration 0500] Residual error : 7583.972930, reg : 3294.634065 + +Results of non-rigid optimization follow. + +.. image:: ../../_static/Advanced/color_map_optimization/non_rigid.png + :width: 300px + +.. image:: ../../_static/Advanced/color_map_optimization/non_rigid_zoom.png + :width: 300px diff --git a/docs/tutorial/Advanced/index.rst b/docs/tutorial/Advanced/index.rst index 82d7931a6d5..2bc9c23c523 100644 --- a/docs/tutorial/Advanced/index.rst +++ b/docs/tutorial/Advanced/index.rst @@ -8,6 +8,7 @@ Advanced fast_global_registration multiway_registration rgbd_integration + color_map_optimization customized_visualization non_blocking_visualization interactive_visualization diff --git a/docs/tutorial/reference.rst b/docs/tutorial/reference.rst index 966ed7219bf..af574bc6f08 100644 --- a/docs/tutorial/reference.rst +++ b/docs/tutorial/reference.rst @@ -13,7 +13,8 @@ Reference .. [Rasu2009] R. Rusu, N. Blodow, and M. Beetz, Fast Point Feature Histograms (FPFH) for 3D registration, ICRA, 2009. .. [Rusinkiewicz2001] S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. In 3-D Digital Imaging and Modeling, 2001. .. [Silberman2012] N. Silberman, D. Hoiem, P. Kohli and R. Fergus, Indoor Segmentation and Support Inference from RGBD Images, ECCV, 2012. -.. [Song2015] S. Song, S. Lichtenberg, and J. Xiao, SUN RGB-D: A RGB-D Scene Understanding Benchmark Suite, CVPR, 2015. +.. [Song2015] S. Song, S. Lichtenberg, and J. Xiao, SUN RGB-D: A RGB-D Scene Understanding Benchmark Suite, CVPR, 2015. .. [Steinbrucker2011] F. Steinbrucker, J. Sturm, and D. Cremers, Real-time visual odometry from dense RGB-D images, In ICCV Workshops, 2011. .. [Strum2012] J. Sturm, N. Engelhard, F. Endres, W. Burgard and D. Cremers, A Benchmark for the Evaluation of RGB-D SLAM Systems, IROS, 2012. +.. [Zhou2014] Q.-Y. Zhou, and V. Koltun, Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras, SIGGRAPH, 2014. .. [Zhou2016] Q.-Y. Zhou, J. Park, and V. Koltun, Fast Global Registration, ECCV, 2016. diff --git a/src/Core/ColorMap/ColorMapOptimization.cpp b/src/Core/ColorMap/ColorMapOptimization.cpp new file mode 100644 index 00000000000..527a25b1af5 --- /dev/null +++ b/src/Core/ColorMap/ColorMapOptimization.cpp @@ -0,0 +1,734 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "ColorMapOptimization.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +// for debugging +#include + +namespace three { + +namespace { + +class ImageWarpingField { + +public: + ImageWarpingField (int width, int height, int number_of_vertical_anchors) { + InitializeWarpingFields(width, height, number_of_vertical_anchors); + } + + void InitializeWarpingFields(int width, int height, + int number_of_vertical_anchors) { + anchor_h_ = number_of_vertical_anchors; + anchor_step_ = (double)height / (anchor_h_ - 1); + anchor_w_ = std::ceil((double)width / anchor_step_) + 1; + flow_ = Eigen::VectorXd(anchor_w_ * anchor_h_ * 2); + for (int i = 0; i <= (anchor_w_ - 1); i++) { + for (int j = 0; j <= (anchor_h_ - 1); j++) { + int baseidx = (i + j * anchor_w_) * 2; + flow_(baseidx) = i * anchor_step_; + flow_(baseidx + 1) = j * anchor_step_; + } + } + } + + Eigen::Vector2d QueryFlow(int i, int j) const { + int baseidx = (i + j * anchor_w_) * 2; + // exceptional case: quried anchor index is out of pre-defined space + if (baseidx < 0 || baseidx > anchor_w_ * anchor_h_ * 2) + return Eigen::Vector2d(0.0, 0.0); + else + return Eigen::Vector2d(flow_(baseidx), flow_(baseidx + 1)); + } + + Eigen::Vector2d GetImageWarpingField(double u, double v) const { + int i = (int)(u / anchor_step_); + int j = (int)(v / anchor_step_); + double p = (u - i * anchor_step_) / anchor_step_; + double q = (v - j * anchor_step_) / anchor_step_; + return (1 - p) * (1 - q) * QueryFlow(i, j) + + (1 - p) * (q)* QueryFlow(i, j + 1) + + (p)* (1 - q) * QueryFlow(i + 1, j) + + (p)* (q)* QueryFlow(i + 1, j + 1); + } + +public: + Eigen::VectorXd flow_; + int anchor_w_, anchor_h_; + double anchor_step_; +}; + +const double IMAGE_BOUNDARY_MARGIN = 10; + +inline std::tuple Project3DPointAndGetUVDepth( + const Eigen::Vector3d X, + const PinholeCameraTrajectory& camera, int camid) +{ + std::pair f = camera.intrinsic_.GetFocalLength(); + std::pair p = camera.intrinsic_.GetPrincipalPoint(); + Eigen::Vector4d Vt = camera.extrinsic_[camid] * + Eigen::Vector4d(X(0), X(1), X(2), 1); + float u = (Vt(0) * f.first) / Vt(2) + p.first; + float v = (Vt(1) * f.second) / Vt(2) + p.second; + float z = Vt(2); + return std::make_tuple(u, v, z); +} + +std::tuple>, std::vector>> + MakeVertexAndImageVisibility(const TriangleMesh& mesh, + const std::vector& images_rgbd, + const std::vector& images_mask, + const PinholeCameraTrajectory& camera, + const ColorMapOptmizationOption& option) +{ + auto n_camera = camera.extrinsic_.size(); + auto n_vertex = mesh.vertices_.size(); + std::vector> visiblity_vertex_to_image; + std::vector> visiblity_image_to_vertex; + visiblity_vertex_to_image.resize(n_vertex); + visiblity_image_to_vertex.resize(n_camera); +#ifdef _OPENMP +#pragma omp parallel for schedule(static) +#endif + for (int c = 0; c < n_camera; c++) { + int viscnt = 0; + for (int vertex_id = 0; vertex_id < n_vertex; vertex_id++) { + Eigen::Vector3d X = mesh.vertices_[vertex_id]; + double u, v, d; + std::tie(u, v, d) = Project3DPointAndGetUVDepth(X, camera, c); + int u_d = round(u), v_d = round(v); + if (d < 0.0 || !images_rgbd[c].depth_.TestImageBoundary(u_d, v_d)) + continue; + float d_sensor = *PointerAt(images_rgbd[c].depth_, u_d, v_d); + if (d_sensor > option.maximum_allowable_depth_) + continue; + if (*PointerAt(images_mask[c], u_d, v_d) == 255) + continue; + if (abs(d - d_sensor) < + option.depth_threshold_for_visiblity_check_) { +#ifdef _OPENMP +#pragma omp critical +#endif + { + visiblity_vertex_to_image[vertex_id].push_back(c); + visiblity_image_to_vertex[c].push_back(vertex_id); + viscnt++; + } + } + } + PrintDebug("[cam %d] %.5f percents are visible\n", + c, double(viscnt) / n_vertex * 100); fflush(stdout); + } + return std::move(std::make_tuple( + visiblity_vertex_to_image, visiblity_image_to_vertex)); +} + +std::vector MakeWarpingFields( + const std::vector>& images, + const ColorMapOptmizationOption& option) +{ + std::vector fields; + for (auto i = 0; i < images.size(); i++) { + int width = images[i]->width_; + int height = images[i]->height_; + fields.push_back(ImageWarpingField(width, height, + option.number_of_vertical_anchors_)); + } + return std::move(fields); +} + +template +std::tuple QueryImageIntensity( + const Image& img, const Eigen::Vector3d& V, + const PinholeCameraTrajectory& camera, int camid, int ch = -1) +{ + float u, v, depth; + std::tie(u, v, depth) = Project3DPointAndGetUVDepth(V, camera, camid); + if (img.TestImageBoundary(u, v, IMAGE_BOUNDARY_MARGIN)) { + if (ch == -1) { + return std::make_tuple(true, *PointerAt(img, u, v)); + } else { + return std::make_tuple(true, *PointerAt(img, u, v, ch)); + } + } else { + return std::make_tuple(false, 0); + } +} + +template +std::tuple QueryImageIntensity( + const Image& img, const ImageWarpingField& field, + const Eigen::Vector3d& V, + const PinholeCameraTrajectory& camera, int camid, int ch = -1) +{ + float u, v, depth; + std::tie(u, v, depth) = Project3DPointAndGetUVDepth(V, camera, camid); + if (img.TestImageBoundary(u, v, IMAGE_BOUNDARY_MARGIN)) { + Eigen::Vector2d uv_shift = field.GetImageWarpingField(u, v); + if (img.TestImageBoundary(uv_shift(0), uv_shift(1), + IMAGE_BOUNDARY_MARGIN)) { + if (ch == -1) { + return std::make_tuple(true, + *PointerAt(img, uv_shift(0), uv_shift(1))); + } else { + return std::make_tuple(true, + *PointerAt(img, uv_shift(0), uv_shift(1), ch)); + } + } + } + return std::make_tuple(false, 0); +} + +void SetProxyIntensityForVertex(const TriangleMesh& mesh, + const std::vector>& images_gray, + const std::vector& warping_field, + const PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image, + std::vector& proxy_intensity) +{ + auto n_vertex = mesh.vertices_.size(); + proxy_intensity.resize(n_vertex); + +#pragma omp parallel for schedule(static) + for (auto i = 0; i < n_vertex; i++) { + proxy_intensity[i] = 0.0; + float sum = 0.0; + for (auto iter = 0; iter < visiblity_vertex_to_image[i].size(); + iter++) { + int j = visiblity_vertex_to_image[i][iter]; + float gray; + bool valid = false; + std::tie(valid, gray) = QueryImageIntensity( + *images_gray[j], warping_field[j], + mesh.vertices_[i], camera, j); + if (valid) { + sum += 1.0; + proxy_intensity[i] += gray; + } + } + if (sum > 0) { + proxy_intensity[i] /= sum; + } + } +} + +void SetProxyIntensityForVertex(const TriangleMesh& mesh, + const std::vector>& images_gray, + const PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image, + std::vector& proxy_intensity) +{ + auto n_vertex = mesh.vertices_.size(); + proxy_intensity.resize(n_vertex); + +#pragma omp parallel for num_threads( 8 ) + for (auto i = 0; i < n_vertex; i++) { + proxy_intensity[i] = 0.0; + float sum = 0.0; + for (auto iter = 0; iter < visiblity_vertex_to_image[i].size(); + iter++) { + int j = visiblity_vertex_to_image[i][iter]; + float gray; + bool valid = false; + std::tie(valid, gray) = QueryImageIntensity( + *images_gray[j], mesh.vertices_[i], camera, j); + if (valid) { + sum += 1.0; + proxy_intensity[i] += gray; + } + } + if (sum > 0) { + proxy_intensity[i] /= sum; + } + } +} + +void OptimizeImageCoorNonrigid( + const TriangleMesh& mesh, + const std::vector>& images_gray, + const std::vector>& images_dx, + const std::vector>& images_dy, + std::vector& warping_fields, + const std::vector& warping_fields_init, + PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image, + const std::vector>& visiblity_image_to_vertex, + std::vector& proxy_intensity, + const ColorMapOptmizationOption& option) +{ + auto n_vertex = mesh.vertices_.size(); + auto n_camera = camera.extrinsic_.size(); + Eigen::Matrix4d intr = Eigen::Matrix4d::Zero(); + intr.block<3,3>(0,0) = camera.intrinsic_.intrinsic_matrix_; + intr(3, 3) = 1.0; + double fx = intr(0, 0); + double fy = intr(1, 1); + SetProxyIntensityForVertex(mesh, images_gray, warping_fields, camera, + visiblity_vertex_to_image, proxy_intensity); + for (int itr = 0; itr < option.maximum_iteration_; itr++) { + PrintDebug("[Iteration %04d] ", itr+1); + double residual = 0.0; + double residual_reg = 0.0; + int total_num_ = 0; +#ifdef _OPENMP +#pragma omp parallel for schedule(static) +#endif + for (int i = 0; i < n_camera; i++) { + int nonrigidval = warping_fields[i].anchor_w_ * + warping_fields[i].anchor_h_ * 2; + Eigen::MatrixXd JJ = Eigen::MatrixXd::Zero( + 6 + nonrigidval, 6 + nonrigidval); + Eigen::VectorXd Jb = Eigen::VectorXd::Zero(6 + nonrigidval); + double rr = 0.0; + double rr_reg = 0.0; + int this_num = 0; + Eigen::Matrix4d pose; + pose = camera.extrinsic_[i]; + double anchor_step = warping_fields[i].anchor_step_; + double anchor_w = warping_fields[i].anchor_w_; + for (auto iter = 0; iter < visiblity_image_to_vertex[i].size(); + iter++) { + int j = visiblity_image_to_vertex[i][iter]; + Eigen::Vector3d V = mesh.vertices_[j]; + Eigen::Vector4d G = pose * Eigen::Vector4d(V(0), V(1), V(2), 1); + Eigen::Vector4d L = intr * G; + double u = L(0) / L(2); + double v = L(1) / L(2); + int ii = (int)(u / anchor_step); + int jj = (int)(v / anchor_step); + double p = (u - ii * anchor_step) / anchor_step; + double q = (v - jj * anchor_step) / anchor_step; + Eigen::Vector2d grids[4] = { + warping_fields[i].QueryFlow(ii, jj), + warping_fields[i].QueryFlow(ii, jj + 1), + warping_fields[i].QueryFlow(ii + 1, jj), + warping_fields[i].QueryFlow(ii + 1, jj + 1), + }; + Eigen::Vector2d uuvv = (1 - p) * (1 - q) * grids[0] + + (1 - p) * (q)* grids[1] + + (p)* (1 - q) * grids[2] + + (p)* (q)* grids[3]; + float uu = uuvv(0); + float vv = uuvv(1); + if (!images_gray[i]->TestImageBoundary(uu, vv, + IMAGE_BOUNDARY_MARGIN)) + continue; + double gray = *PointerAt(*images_gray[i], uu, vv); + Eigen::Vector2d dIdf(*PointerAt(*images_dx[i], uu, vv), + *PointerAt(*images_dy[i], uu, vv)); + Eigen::Vector2d dfdx = ((grids[2] - grids[0]) * (1 - q) + + (grids[3] - grids[1]) * q) / anchor_step; + Eigen::Vector2d dfdy = ((grids[1] - grids[0]) * (1 - p) + + (grids[3] - grids[2]) * p) / anchor_step; + double dIdx = dIdf.dot(dfdx); + double dIdy = dIdf.dot(dfdy); + double invz = 1. / G(2); + double v0 = dIdx * fx * invz; + double v1 = dIdy * fy * invz; + double v2 = -(v0 * G(0) + v1 * G(1)) * invz; + double C[6 + 8]; + C[0] = -G(2) * v1 + G(1) * v2; + C[1] = G(2) * v0 - G(0) * v2; + C[2] = -G(1) * v0 + G(0) * v1; + C[3] = v0; + C[4] = v1; + C[5] = v2; + C[6] = dIdf(0) * (1 - p) * (1 - q); + C[7] = dIdf(1) * (1 - p) * (1 - q); + C[8] = dIdf(0) * (1 - p) * (q); + C[9] = dIdf(1) * (1 - p) * (q); + C[10] = dIdf(0) * (p)* (1 - q); + C[11] = dIdf(1) * (p)* (1 - q); + C[12] = dIdf(0) * (p)* (q); + C[13] = dIdf(1) * (p)* (q); + int idx[6 + 8]; + idx[0] = 0; + idx[1] = 1; + idx[2] = 2; + idx[3] = 3; + idx[4] = 4; + idx[5] = 5; + idx[6] = 6 + (ii + jj * anchor_w) * 2; + idx[7] = 6 + (ii + jj * anchor_w) * 2 + 1; + idx[8] = 6 + (ii + (jj + 1) * anchor_w) * 2; + idx[9] = 6 + (ii + (jj + 1) * anchor_w) * 2 + 1; + idx[10] = 6 + ((ii + 1) + jj * anchor_w) * 2; + idx[11] = 6 + ((ii + 1) + jj * anchor_w) * 2 + 1; + idx[12] = 6 + ((ii + 1) + (jj + 1) * anchor_w) * 2; + idx[13] = 6 + ((ii + 1) + (jj + 1) * anchor_w) * 2 + 1; + for (int x = 0; x < 14; x++) { + for (int y = 0; y < 14; y++) { + JJ(idx[x], idx[y]) += C[x] * C[y]; + } + } + double r = (proxy_intensity[j] - gray); + for (int x = 0; x < 14; x++) { + Jb(idx[x]) -= r * C[x]; + } + rr += r * r; + this_num++; + } + if (this_num == 0) + continue; + double weight = option.non_rigid_anchor_point_weight_ + * this_num / n_vertex; + for (int j = 0; j < nonrigidval; j++) { + double r = weight * (warping_fields[i].flow_(j) - + warping_fields_init[i].flow_(j)); + JJ(6 + j, 6 + j) += weight * weight; + Jb(6 + j) += weight * r; + rr_reg += r * r; + } +#ifdef _OPENMP +#pragma omp critical +#endif + { + bool success = false; + Eigen::VectorXd result; + std::tie(success, result) = SolveLinearSystem(JJ, -Jb, false); + Eigen::Affine3d aff_mat; + aff_mat.linear() = (Eigen::Matrix3d) + Eigen::AngleAxisd(result(2),Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(result(1),Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(result(0),Eigen::Vector3d::UnitX()); + aff_mat.translation() = + Eigen::Vector3d(result(3), result(4), result(5)); + pose = aff_mat.matrix() * pose; + for (int j = 0; j < nonrigidval; j++) { + warping_fields[i].flow_(j) += result(6 + j); + } + } + camera.extrinsic_[i] = pose; + residual += rr; + residual_reg += rr_reg; + total_num_ += this_num; + } + PrintDebug("Residual error : %.6f, reg : %.6f\n", + residual, residual_reg); + SetProxyIntensityForVertex(mesh, images_gray, warping_fields, camera, + visiblity_vertex_to_image, proxy_intensity); + } +} + +void OptimizeImageCoorRigid( + const TriangleMesh& mesh, + const std::vector>& images_gray, + const std::vector>& images_dx, + const std::vector>& images_dy, + PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image, + const std::vector>& visiblity_image_to_vertex, + std::vector& proxy_intensity, + const ColorMapOptmizationOption& option) +{ + int total_num_ = 0; + auto n_camera = camera.extrinsic_.size(); + Eigen::Matrix4d intr = Eigen::Matrix4d::Zero(); + intr.block<3,3>(0,0) = camera.intrinsic_.intrinsic_matrix_; + intr(3, 3) = 1.0; + double fx = intr(0, 0); + double fy = intr(1, 1); + SetProxyIntensityForVertex(mesh, images_gray, camera, + visiblity_vertex_to_image, proxy_intensity); + for (int itr = 0; itr < option.maximum_iteration_; itr++) { + PrintDebug("[Iteration %04d] ", itr+1); + double residual = 0.0; + total_num_ = 0; +#ifdef _OPENMP +#pragma omp parallel for schedule(static) +#endif + for (int i = 0; i < n_camera; i++) { + Eigen::MatrixXd JJ(6, 6); + Eigen::VectorXd Jb(6); + JJ.setZero(); + Jb.setZero(); + double rr = 0.0; + int this_num = 0; + Eigen::Matrix4d pose; + pose = camera.extrinsic_[i]; + for (auto iter = 0; iter < visiblity_image_to_vertex[i].size(); + iter++) { + int j = visiblity_image_to_vertex[i][iter]; + Eigen::Vector3d V = mesh.vertices_[j]; + Eigen::Vector4d G = pose * Eigen::Vector4d(V(0), V(1), V(2), 1); + Eigen::Vector4d L = intr * G; + float u = L(0) / L(2); + float v = L(1) / L(2); + if (!images_gray[i]->TestImageBoundary(u, v, + IMAGE_BOUNDARY_MARGIN)) + continue; + double gray = *PointerAt(*images_gray[i], u, v); + if (gray == -1.0) + continue; + double dIdx = *PointerAt(*images_dx[i], u, v); + double dIdy = *PointerAt(*images_dy[i], u, v); + double invz = 1. / G(2); + double v0 = dIdx * fx * invz; + double v1 = dIdy * fy * invz; + double v2 = -(v0 * G(0) + v1 * G(1)) * invz; + double C[6]; + C[0] = (-G(2) * v1 + G(1) * v2); + C[1] = (G(2) * v0 - G(0) * v2); + C[2] = (-G(1) * v0 + G(0) * v1); + C[3] = v0; + C[4] = v1; + C[5] = v2; + for (int x = 0; x < 6; x++) { + for (int y = 0; y < 6; y++) { + JJ(x, y) += C[x] * C[y]; + } + } + double r = (proxy_intensity[j] - gray); + for (int x = 0; x < 6; x++) { + Jb(x) -= r * C[x]; + } + rr += r * r; + this_num++; + } + Eigen::VectorXd result(6); + result = -JJ.inverse() * Jb; + Eigen::Affine3d aff_mat; + aff_mat.linear() = (Eigen::Matrix3d) + Eigen::AngleAxisd(result(2), Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(result(1), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(result(0), Eigen::Vector3d::UnitX()); + aff_mat.translation() = + Eigen::Vector3d(result(3), result(4), result(5)); + pose = aff_mat.matrix() * pose; + camera.extrinsic_[i] = pose; +#ifdef _OPENMP +#pragma omp critical +#endif + { + residual += rr; + total_num_ += this_num; + } + } + PrintDebug("Residual error : %.6f (avg : %.6f)\n", + residual, residual / total_num_); + SetProxyIntensityForVertex(mesh, images_gray, camera, + visiblity_vertex_to_image, proxy_intensity); + } +} + +void SetGeometryColorAverage(TriangleMesh& mesh, + const std::vector& images_rgbd, + const std::vector& warping_fields, + const PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image) +{ + auto n_vertex = mesh.vertices_.size(); + mesh.vertex_colors_.clear(); + mesh.vertex_colors_.resize(n_vertex); +#pragma omp parallel for schedule(static) + for (int i = 0; i < n_vertex; i++) { + mesh.vertex_colors_[i] = Eigen::Vector3d::Zero(); + double sum = 0.0; + for (auto iter = 0; iter < visiblity_vertex_to_image[i].size(); + iter++) { + int j = visiblity_vertex_to_image[i][iter]; + unsigned char r_temp, g_temp, b_temp; + bool valid = false; + std::tie(valid, r_temp) = QueryImageIntensity( + images_rgbd[j].color_, warping_fields[j], + mesh.vertices_[i], camera, j, 0); + std::tie(valid, g_temp) = QueryImageIntensity( + images_rgbd[j].color_, warping_fields[j], + mesh.vertices_[i], camera, j, 1); + std::tie(valid, b_temp) = QueryImageIntensity( + images_rgbd[j].color_, warping_fields[j], + mesh.vertices_[i], camera, j, 2); + float r = (float)r_temp / 255.0f; + float g = (float)g_temp / 255.0f; + float b = (float)b_temp / 255.0f; + if (valid) { + mesh.vertex_colors_[i] += Eigen::Vector3d(r, g, b); + sum += 1.0; + } + } + if (sum > 0.0) { + mesh.vertex_colors_[i] /= sum; + } + } +} + +void SetGeometryColorAverage(TriangleMesh& mesh, + const std::vector& images_rgbd, + const PinholeCameraTrajectory& camera, + const std::vector>& visiblity_vertex_to_image) +{ + auto n_vertex = mesh.vertices_.size(); + mesh.vertex_colors_.clear(); + mesh.vertex_colors_.resize(n_vertex); +#pragma omp parallel for schedule(static) + for (int i = 0; i < n_vertex; i++) { + mesh.vertex_colors_[i] = Eigen::Vector3d::Zero(); + double sum = 0.0; + for (auto iter = 0; iter < visiblity_vertex_to_image[i].size(); + iter++) { + int j = visiblity_vertex_to_image[i][iter]; + unsigned char r_temp, g_temp, b_temp; + bool valid = false; + std::tie(valid, r_temp) = QueryImageIntensity( + images_rgbd[j].color_, mesh.vertices_[i], camera, j, 0); + std::tie(valid, g_temp) = QueryImageIntensity( + images_rgbd[j].color_, mesh.vertices_[i], camera, j, 1); + std::tie(valid, b_temp) = QueryImageIntensity( + images_rgbd[j].color_, mesh.vertices_[i], camera, j, 2); + float r = (float)r_temp / 255.0f; + float g = (float)g_temp / 255.0f; + float b = (float)b_temp / 255.0f; + if (valid) { + mesh.vertex_colors_[i] += Eigen::Vector3d(r, g, b); + sum += 1.0; + } + } + if (sum > 0.0) { + mesh.vertex_colors_[i] /= sum; + } + } +} + +std::tuple>, + std::vector>, + std::vector>> MakeGradientImages( + const std::vector& images_rgbd) +{ + auto n_images = images_rgbd.size(); + std::vector> images_gray; + std::vector> images_dx; + std::vector> images_dy; + for (int i=0; i MakeDepthMasks( + const std::vector& images_rgbd, + const ColorMapOptmizationOption& option) +{ + auto n_images = images_rgbd.size(); + std::vector images_mask; + for (int i=0; i(); + mask->PrepareImage(width, height, 1, 1); + for (int v=0; v(*depth_image_gradient_dx, u, v); + double dy = *PointerAt(*depth_image_gradient_dy, u, v); + double mag = sqrt(dx * dx + dy * dy); + if (mag > option.depth_threshold_for_discontinuity_check_) { + *PointerAt(*mask, u, v) = 255; + } else { + *PointerAt(*mask, u, v) = 0; + } + } + } + auto mask_dilated = DilateImage(*mask, + option.half_dilation_kernel_size_for_discontinuity_map_); + images_mask.push_back(*mask_dilated); + + char filename[255]; + sprintf(filename, "image_%03d.png", i); + WriteImage(filename, *mask_dilated); + + } + return std::move(images_mask); +} + +} // unnamed namespace + +void ColorMapOptimization(TriangleMesh& mesh, + const std::vector& images_rgbd, + PinholeCameraTrajectory& camera, + const ColorMapOptmizationOption& option + /* = ColorMapOptmizationOption()*/) +{ + PrintDebug("[ColorMapOptimization]\n"); + std::vector> images_gray; + std::vector> images_dx; + std::vector> images_dy; + std::tie(images_gray, images_dx, images_dy) = + MakeGradientImages(images_rgbd); + + PrintDebug("[ColorMapOptimization] :: MakingMasks\n"); + auto images_mask = MakeDepthMasks(images_rgbd, option); + + PrintDebug("[ColorMapOptimization] :: VisibilityCheck\n"); + std::vector> visiblity_vertex_to_image; + std::vector> visiblity_image_to_vertex; + std::tie(visiblity_vertex_to_image, visiblity_image_to_vertex) = + MakeVertexAndImageVisibility(mesh, images_rgbd, + images_mask, camera, option); + + std::vector proxy_intensity; + if (option.non_rigid_camera_coordinate_) { + PrintDebug("[ColorMapOptimization] :: Non-Rigid Optimization\n"); + auto warping_uv_ = MakeWarpingFields(images_gray, option); + auto warping_uv_init_ = MakeWarpingFields(images_gray, option); + OptimizeImageCoorNonrigid(mesh, images_gray, + images_dx, images_dy, warping_uv_, warping_uv_init_, camera, + visiblity_vertex_to_image, visiblity_image_to_vertex, + proxy_intensity, option); + SetGeometryColorAverage(mesh, images_rgbd, warping_uv_, camera, + visiblity_vertex_to_image); + } else { + PrintDebug("[ColorMapOptimization] :: Rigid Optimization\n"); + OptimizeImageCoorRigid(mesh, images_gray, images_dx, images_dy, camera, + visiblity_vertex_to_image, visiblity_image_to_vertex, + proxy_intensity, option); + SetGeometryColorAverage(mesh, images_rgbd, camera, + visiblity_vertex_to_image); + } +} + +} // namespace three diff --git a/src/Core/ColorMap/ColorMapOptimization.h b/src/Core/ColorMap/ColorMapOptimization.h new file mode 100644 index 00000000000..4a49a4c7ad0 --- /dev/null +++ b/src/Core/ColorMap/ColorMapOptimization.h @@ -0,0 +1,83 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#pragma once + +#include + +namespace three { + +class TriangleMesh; +class RGBDImage; +class Image; +class PinholeCameraTrajectory; + +class ColorMapOptmizationOption +{ +public: + ColorMapOptmizationOption( + bool non_rigid_camera_coordinate = false, + int number_of_vertical_anchors = 16, + double non_rigid_anchor_point_weight = 0.316, + double maximum_iteration = 500, + double maximum_allowable_depth = 2.5, + double depth_threshold_for_visiblity_check = 0.03, + double depth_threshold_for_discontinuity_check = 0.1, + int half_dilation_kernel_size_for_discontinuity_map = 3) : + non_rigid_camera_coordinate_(non_rigid_camera_coordinate), + number_of_vertical_anchors_(number_of_vertical_anchors), + non_rigid_anchor_point_weight_(non_rigid_anchor_point_weight), + maximum_iteration_(maximum_iteration), + maximum_allowable_depth_(maximum_allowable_depth), + depth_threshold_for_visiblity_check_( + depth_threshold_for_visiblity_check), + depth_threshold_for_discontinuity_check_( + depth_threshold_for_discontinuity_check), + half_dilation_kernel_size_for_discontinuity_map_( + half_dilation_kernel_size_for_discontinuity_map) {} + ~ColorMapOptmizationOption() {} + +public: + bool non_rigid_camera_coordinate_; + int number_of_vertical_anchors_; + double non_rigid_anchor_point_weight_; + double maximum_iteration_; + double maximum_allowable_depth_; + double depth_threshold_for_visiblity_check_; + double depth_threshold_for_discontinuity_check_; + int half_dilation_kernel_size_for_discontinuity_map_; +}; + +/// This is implementation of following paper +/// Q.-Y. Zhou and V. Koltun, +/// Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras, +/// SIGGRAPH 2014 +void ColorMapOptimization(TriangleMesh& mesh, + const std::vector& imgs_rgbd, + PinholeCameraTrajectory& camera, + const ColorMapOptmizationOption& option = + ColorMapOptmizationOption()); +} // namespace three diff --git a/src/Core/Geometry/Image.cpp b/src/Core/Geometry/Image.cpp index 8d3d57e7e0e..9f1cc7377b0 100644 --- a/src/Core/Geometry/Image.cpp +++ b/src/Core/Geometry/Image.cpp @@ -61,6 +61,13 @@ Eigen::Vector2d Image::GetMaxBound() const return Eigen::Vector2d(width_, height_); } +bool Image::TestImageBoundary(double u, double v, + double inner_margin/* = 0.0 */) const +{ + return (u >= inner_margin && u < width_ - inner_margin && + v >= inner_margin && v < height_ - inner_margin); +} + std::pair Image::FloatValueAt(double u, double v) const { if ((num_of_channels_ != 1) || (bytes_per_channel_ != 4) || @@ -296,4 +303,37 @@ std::shared_ptr FlipImage(const Image &input) return output; } +std::shared_ptr DilateImage(const Image &input, + int half_kernel_size/* = 1 */) +{ + auto output = std::make_shared(); + if (input.num_of_channels_ != 1 || input.bytes_per_channel_ != 1) { + PrintWarning("[DilateImage] Unsupported image format.\n"); + return output; + } + output->PrepareImage(input.width_, input.height_, 1, 1); + +#ifdef _OPENMP +#pragma omp parallel for schedule(static) +#endif + for (int y = 0; y < input.height_; y++) { + for (int x = 0; x < input.width_; x++) { + for (int yy = -half_kernel_size; yy <= half_kernel_size; yy++) { + for (int xx = -half_kernel_size; xx <= half_kernel_size; xx++) { + unsigned char* pi; + if (input.TestImageBoundary(x+xx, y+yy)) { + pi = PointerAt(input, x+xx, y+yy); + if (*pi == 255) { + *PointerAt(*output, x, y, 0) = 255; + xx = half_kernel_size; + yy = half_kernel_size; + } + } + } + } + } + } + return output; +} + } // namespace three diff --git a/src/Core/Geometry/Image.h b/src/Core/Geometry/Image.h index 6c13ff56145..a5fab397139 100644 --- a/src/Core/Geometry/Image.h +++ b/src/Core/Geometry/Image.h @@ -62,6 +62,7 @@ class Image : public Geometry2D bool IsEmpty() const override; Eigen::Vector2d GetMinBound() const override; Eigen::Vector2d GetMaxBound() const override; + bool TestImageBoundary(double u, double v, double inner_margin = 0.0) const; public: virtual bool HasData() const { @@ -140,6 +141,10 @@ std::shared_ptr FilterHorizontalImage( /// Function to 2x image downsample using simple 2x2 averaging std::shared_ptr DownsampleImage(const Image &input); +/// Function to dilate 8bit mask map +std::shared_ptr DilateImage(const Image &input, + int half_kernel_size = 1); + /// Function to linearly transform pixel intensities /// image_new = scale * image + offset void LinearTransformImage(Image &input, diff --git a/src/Core/Utility/Eigen.cpp b/src/Core/Utility/Eigen.cpp index 33256c5cbf8..5c0c7335b92 100644 --- a/src/Core/Utility/Eigen.cpp +++ b/src/Core/Utility/Eigen.cpp @@ -33,20 +33,25 @@ namespace three{ /// Function to solve Ax=b std::tuple SolveLinearSystem( - const Eigen::MatrixXd &A, const Eigen::VectorXd &b) + const Eigen::MatrixXd &A, const Eigen::VectorXd &b, + bool check_det/* = true */) { - bool solution_exist = true; - // note: computing determinant for large scale matrix would be bottleneck. - double det = A.determinant(); - if (fabs(det) < 1e-6 || std::isnan(det) || std::isinf(det)) - solution_exist = false; - - if (solution_exist) { - // Robust Cholesky decomposition of a matrix with pivoting. - Eigen::MatrixXd x = A.ldlt().solve(b); - return std::make_tuple(solution_exist, std::move(x)); + if (check_det) { + bool solution_exist = true; + double det = A.determinant(); + if (fabs(det) < 1e-6 || std::isnan(det) || std::isinf(det)) + solution_exist = false; + if (solution_exist) { + // Robust Cholesky decomposition of a matrix with pivoting. + Eigen::MatrixXd x = A.ldlt().solve(b); + return std::make_tuple(solution_exist, std::move(x)); + } else { + return std::make_tuple(false, + std::move(Eigen::VectorXd::Zero(b.rows()))); + } } else { - return std::make_tuple(false, std::move(Eigen::VectorXd::Zero(b.rows()))); + Eigen::MatrixXd x = A.ldlt().solve(b); + return std::make_tuple(true, std::move(x)); } } diff --git a/src/Core/Utility/Eigen.h b/src/Core/Utility/Eigen.h index 3229b3da191..fda7cf72c81 100644 --- a/src/Core/Utility/Eigen.h +++ b/src/Core/Utility/Eigen.h @@ -53,7 +53,7 @@ Eigen::Vector6d TransformMatrix4dToVector6d(const Eigen::Matrix4d &input); /// Function to solve Ax=b std::tuple SolveLinearSystem( - const Eigen::MatrixXd &A, const Eigen::VectorXd &b); + const Eigen::MatrixXd &A, const Eigen::VectorXd &b, bool check_det = true); /// Function to solve Jacobian system /// Input: 6x6 Jacobian matrix and 6-dim residual vector. diff --git a/src/Python/Core/open3d_colormap.cpp b/src/Python/Core/open3d_colormap.cpp new file mode 100644 index 00000000000..227cac311cc --- /dev/null +++ b/src/Python/Core/open3d_colormap.cpp @@ -0,0 +1,80 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "open3d_core.h" +#include "open3d_core_trampoline.h" + +#include +#include +#include +#include + +using namespace three; + +void pybind_colormap_optimization(py::module &m) +{ + py::class_ color_map_optimization_option( + m, "ColorMapOptmizationOption"); + py::detail::bind_default_constructor( + color_map_optimization_option); + color_map_optimization_option + .def_readwrite("non_rigid_camera_coordinate", + &ColorMapOptmizationOption::non_rigid_camera_coordinate_) + .def_readwrite("number_of_vertical_anchors", + &ColorMapOptmizationOption::number_of_vertical_anchors_) + .def_readwrite("non_rigid_anchor_point_weight", + &ColorMapOptmizationOption::non_rigid_anchor_point_weight_) + .def_readwrite("maximum_iteration", + &ColorMapOptmizationOption::maximum_iteration_) + .def_readwrite("maximum_allowable_depth", + &ColorMapOptmizationOption::maximum_allowable_depth_) + .def_readwrite("depth_threshold_for_visiblity_check", + &ColorMapOptmizationOption::depth_threshold_for_visiblity_check_) + .def("__repr__", [](const ColorMapOptmizationOption &to) { + return std::string("ColorMapOptmizationOption with") + + std::string("\n- non_rigid_camera_coordinate : ") + + std::to_string(to.non_rigid_camera_coordinate_) + + std::string("\n- number_of_vertical_anchors : ") + + std::to_string(to.number_of_vertical_anchors_) + + std::string("\n- non_rigid_anchor_point_weight : ") + + std::to_string(to.non_rigid_anchor_point_weight_) + + std::string("\n- maximum_iteration : ") + + std::to_string(to.maximum_iteration_) + + std::string("\n- maximum_allowable_depth : ") + + std::to_string(to.maximum_allowable_depth_) + + std::string("\n- depth_threshold_for_visiblity_check : ") + + std::to_string(to.depth_threshold_for_visiblity_check_); + }); +} + +void pybind_colormap_optimization_methods(py::module &m) +{ + m.def("color_map_optimization", + &ColorMapOptimization, + "Function for color mapping of reconstructed scenes via optimization", + "mesh"_a, "imgs_rgbd"_a, "camera"_a, + "option"_a = ColorMapOptmizationOption()); +} diff --git a/src/Python/Core/open3d_core.cpp b/src/Python/Core/open3d_core.cpp index 749ac5ce7b0..916d6d4b690 100644 --- a/src/Python/Core/open3d_core.cpp +++ b/src/Python/Core/open3d_core.cpp @@ -38,8 +38,9 @@ void pybind_core_classes(py::module &m) pybind_camera(m); pybind_registration(m); pybind_odometry(m); - pybind_globaloptimization(m); + pybind_global_optimization(m); pybind_integration(m); + pybind_colormap_optimization(m); } void pybind_core_methods(py::module &m) @@ -51,6 +52,7 @@ void pybind_core_methods(py::module &m) pybind_camera_methods(m); pybind_registration_methods(m); pybind_odometry_methods(m); - pybind_globaloptimization_methods(m); + pybind_global_optimization_methods(m); pybind_integration_methods(m); + pybind_colormap_optimization_methods(m); } diff --git a/src/Python/Core/open3d_core.h b/src/Python/Core/open3d_core.h index 869005bc1ef..119e98952dd 100644 --- a/src/Python/Core/open3d_core.h +++ b/src/Python/Core/open3d_core.h @@ -38,8 +38,9 @@ void pybind_feature(py::module &m); void pybind_camera(py::module &m); void pybind_registration(py::module &m); void pybind_odometry(py::module &m); -void pybind_globaloptimization(py::module &m); +void pybind_global_optimization(py::module &m); void pybind_integration(py::module &m); +void pybind_colormap_optimization(py::module &m); void pybind_pointcloud_methods(py::module &m); void pybind_trianglemesh_methods(py::module &m); @@ -48,5 +49,6 @@ void pybind_feature_methods(py::module &m); void pybind_camera_methods(py::module &m); void pybind_registration_methods(py::module &m); void pybind_odometry_methods(py::module &m); -void pybind_globaloptimization_methods(py::module &m); +void pybind_global_optimization_methods(py::module &m); void pybind_integration_methods(py::module &m); +void pybind_colormap_optimization_methods(py::module &m); diff --git a/src/Python/Core/open3d_globaloptimization.cpp b/src/Python/Core/open3d_global_optimization.cpp similarity index 98% rename from src/Python/Core/open3d_globaloptimization.cpp rename to src/Python/Core/open3d_global_optimization.cpp index ef06a26afb2..01463d49c94 100644 --- a/src/Python/Core/open3d_globaloptimization.cpp +++ b/src/Python/Core/open3d_global_optimization.cpp @@ -51,7 +51,7 @@ class PyGlobalOptimizationMethod : public GlobalOptimizationMethodBase } }; -void pybind_globaloptimization(py::module &m) +void pybind_global_optimization(py::module &m) { py::class_> pose_graph_node(m, "PoseGraphNode"); @@ -208,7 +208,7 @@ void pybind_globaloptimization(py::module &m) return new GlobalOptimizationOption(max_correspondence_distance, edge_prune_threshold, preference_loop_closure, reference_node); }), "max_correspondence_distance"_a = 0.03, - "edge_prune_threshold"_a = 0.25, + "edge_prune_threshold"_a = 0.25, "preference_loop_closure"_a = 1.0, "reference_node"_a = -1) .def("__repr__", [](const GlobalOptimizationOption &goo) { return std::string("GlobalOptimizationOption") + @@ -223,7 +223,7 @@ void pybind_globaloptimization(py::module &m) }); } -void pybind_globaloptimization_methods(py::module &m) +void pybind_global_optimization_methods(py::module &m) { m.def("read_pose_graph", [](const std::string &filename) { PoseGraph pose_graph; diff --git a/src/Python/Tutorial/Advanced/color_map_optimization.py b/src/Python/Tutorial/Advanced/color_map_optimization.py new file mode 100644 index 00000000000..2d19e1fcb0e --- /dev/null +++ b/src/Python/Tutorial/Advanced/color_map_optimization.py @@ -0,0 +1,58 @@ +# Open3D: www.open3d.org +# The MIT License (MIT) +# See license file or visit www.open3d.org for details + +from open3d import * +from trajectory_io import * +import os, sys +sys.path.append("../Utility") +from common import * + +path = "[path_to_fountain_dataset]" +debug_mode = False + +if __name__ == "__main__": + set_verbosity_level(VerbosityLevel.Debug) + + # Read RGBD images + rgbd_images = [] + depth_image_path = get_file_list( + os.path.join(path, "depth/"), extension = ".png") + color_image_path = get_file_list( + os.path.join(path, "image/"), extension = ".jpg") + assert(len(depth_image_path) == len(color_image_path)) + for i in range(len(depth_image_path)): + depth = read_image(os.path.join(depth_image_path[i])) + color = read_image(os.path.join(color_image_path[i])) + rgbd_image = create_rgbd_image_from_color_and_depth(color, depth, + convert_rgb_to_intensity = False) + if debug_mode: + pcd = create_point_cloud_from_rgbd_image(rgbd_image, + PinholeCameraIntrinsic.get_prime_sense_default()) + draw_geometries([pcd]) + rgbd_images.append(rgbd_image) + + # Read camera pose and mesh + camera = read_pinhole_camera_trajectory(os.path.join(path, "scene/key.log")) + mesh = read_triangle_mesh(os.path.join(path, "scene", "integrated.ply")) + + # Before full optimization, let's just visualize texture map + # with given geometry, RGBD images, and camera poses. + option = ColorMapOptmizationOption() + option.maximum_iteration = 0 + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_before_optimization.ply"), mesh) + + # Optimize texture and save the mesh as texture_mapped.ply + # This is implementation of following paper + # Q.-Y. Zhou and V. Koltun, + # Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras, + # SIGGRAPH 2014 + option.maximum_iteration = 500 + option.non_rigid_camera_coordinate = True + color_map_optimization(mesh, rgbd_images, camera, option) + draw_geometries([mesh]) + write_triangle_mesh(os.path.join(path, "scene", + "color_map_after_optimization.ply"), mesh) diff --git a/src/Python/Tutorial/Misc/sampling.py b/src/Python/Tutorial/Misc/sampling.py new file mode 100644 index 00000000000..b10b56becc9 --- /dev/null +++ b/src/Python/Tutorial/Misc/sampling.py @@ -0,0 +1,57 @@ +# Open3D: www.open3d.org +# The MIT License (MIT) +# See license file or visit www.open3d.org for details + +from open3d import * +import os, sys +sys.path.append("../Utility") +from common import * +sys.path.append("../Advanced") +from trajectory_io import * +from shutil import copyfile + +if __name__ == "__main__": + set_verbosity_level(VerbosityLevel.Debug) + + path = "[path_to_reconstruction_system_output]" + out_path = "[path_to_sampled_frames_are_located]" + make_folder(out_path) + make_folder(out_path + "depth/") + make_folder(out_path + "image/") + make_folder(out_path + "scene/") + sampling_rate = 30 + + depth_image_path = get_file_list( + os.path.join(path, "depth/"), extension = ".png") + color_image_path = get_file_list( + os.path.join(path, "image/"), extension = ".jpg") + pose_graph_global = read_pose_graph(path + + template_global_posegraph_optimized) + n_fragments = len(depth_image_path) // n_frames_per_fragment + 1 + pose_graph_fragments = [] + for i in range(n_fragments): + pose_graph_fragment = read_pose_graph(path + + template_fragment_posegraph_optimized % i) + pose_graph_fragments.append(pose_graph_fragment) + + depth_image_path_new = [] + color_image_path_new = [] + traj = [] + cnt = 0 + for i in range(len(depth_image_path)): + if i % sampling_rate == 0: + metadata = [cnt, cnt, len(depth_image_path) // sampling_rate + 1] + print(metadata) + fragment_id = i // n_frames_per_fragment + local_frame_id = i - fragment_id * n_frames_per_fragment + traj.append(CameraPose(metadata, np.dot( + pose_graph_global.nodes[fragment_id].pose, + pose_graph_fragments[fragment_id].nodes[local_frame_id].pose))) + copyfile(depth_image_path[i], out_path + "depth/" + \ + os.path.basename(depth_image_path[i])) + copyfile(color_image_path[i], out_path + "image/" + \ + os.path.basename(color_image_path[i])) + cnt += 1 + copyfile(path + "/scene/cropped.ply", + out_path + "/scene/integrated.ply") + write_trajectory(traj, out_path + "scene/key.log")