// ---------------------------------------------------------------------------- // - Open3D: www.open3d.org - // ---------------------------------------------------------------------------- // Copyright (c) 2018-2023 www.open3d.org // SPDX-License-Identifier: MIT // ---------------------------------------------------------------------------- #include "open3d/geometry/PointCloud.h" #include #include "open3d/camera/PinholeCameraIntrinsic.h" #include "open3d/data/Dataset.h" #include "open3d/geometry/BoundingVolume.h" #include "open3d/geometry/Image.h" #include "open3d/geometry/RGBDImage.h" #include "open3d/geometry/TriangleMesh.h" #include "open3d/io/ImageIO.h" #include "open3d/io/PinholeCameraTrajectoryIO.h" #include "open3d/io/PointCloudIO.h" #include "open3d/utility/Random.h" #include "open3d/visualization/utility/DrawGeometry.h" #include "tests/Tests.h" namespace open3d { namespace tests { TEST(PointCloud, ConstructorDefault) { geometry::PointCloud pcd; EXPECT_EQ(geometry::Geometry::GeometryType::PointCloud, pcd.GetGeometryType()); EXPECT_EQ(pcd.Dimension(), 3); EXPECT_EQ(pcd.points_.size(), 0); EXPECT_EQ(pcd.normals_.size(), 0); EXPECT_EQ(pcd.colors_.size(), 0); EXPECT_EQ(pcd.covariances_.size(), 0); } TEST(PointCloud, ConstructorFromPoints) { std::vector points = {{0, 1, 2}, {3, 4, 5}}; geometry::PointCloud pcd(points); EXPECT_EQ(pcd.points_.size(), 2); EXPECT_EQ(pcd.normals_.size(), 0); EXPECT_EQ(pcd.colors_.size(), 0); EXPECT_EQ(pcd.covariances_.size(), 0); ExpectEQ(pcd.points_, points); } TEST(PointCloud, Clear_IsEmpty) { std::vector points = {{0, 1, 2}, {3, 4, 5}}; std::vector normals = {{0, 1, 2}, {3, 4, 5}}; std::vector colors = {{0.0, 0.1, 0.2}, {0.3, 0.4, 0.5}}; std::vector covariances = {Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity()}; geometry::PointCloud pcd; pcd.points_ = points; pcd.normals_ = normals; pcd.colors_ = colors; pcd.covariances_ = covariances; EXPECT_FALSE(pcd.IsEmpty()); EXPECT_EQ(pcd.points_.size(), 2); EXPECT_EQ(pcd.normals_.size(), 2); EXPECT_EQ(pcd.colors_.size(), 2); EXPECT_EQ(pcd.covariances_.size(), 2); pcd.Clear(); EXPECT_TRUE(pcd.IsEmpty()); EXPECT_EQ(pcd.points_.size(), 0); EXPECT_EQ(pcd.normals_.size(), 0); EXPECT_EQ(pcd.colors_.size(), 0); EXPECT_EQ(pcd.covariances_.size(), 0); } TEST(PointCloud, GetMinBound) { geometry::PointCloud pcd({{1, 10, 20}, {30, 2, 40}, {50, 60, 3}}); ExpectEQ(pcd.GetMinBound(), Eigen::Vector3d(1, 2, 3)); geometry::PointCloud pc_empty; ExpectEQ(pc_empty.GetMinBound(), Eigen::Vector3d(0, 0, 0)); } TEST(PointCloud, GetMaxBound) { geometry::PointCloud pcd({{1, 10, 20}, {30, 2, 40}, {50, 60, 3}}); ExpectEQ(pcd.GetMaxBound(), Eigen::Vector3d(50, 60, 40)); geometry::PointCloud pc_empty; ExpectEQ(pc_empty.GetMaxBound(), Eigen::Vector3d(0, 0, 0)); } TEST(PointCloud, GetCenter) { geometry::PointCloud pcd({{0, 1, 2}, {3, 4, 5}, {6, 7, 8}, {9, 10, 11}}); ExpectEQ(pcd.GetCenter(), Eigen::Vector3d(4.5, 5.5, 6.5)); geometry::PointCloud pc_empty; ExpectEQ(pc_empty.GetCenter(), Eigen::Vector3d(0, 0, 0)); } TEST(PointCloud, GetAxisAlignedBoundingBox) { geometry::PointCloud pcd; geometry::AxisAlignedBoundingBox aabb; pcd = geometry::PointCloud(); aabb = pcd.GetAxisAlignedBoundingBox(); EXPECT_EQ(aabb.min_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.max_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.color_, Eigen::Vector3d(1, 1, 1)); pcd = geometry::PointCloud({{0, 0, 0}}); aabb = pcd.GetAxisAlignedBoundingBox(); EXPECT_EQ(aabb.min_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.max_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.color_, Eigen::Vector3d(1, 1, 1)); pcd = geometry::PointCloud({{0, 2, 0}, {1, 1, 2}, {1, 0, 3}}); aabb = pcd.GetAxisAlignedBoundingBox(); EXPECT_EQ(aabb.min_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.max_bound_, Eigen::Vector3d(1, 2, 3)); EXPECT_EQ(aabb.color_, Eigen::Vector3d(1, 1, 1)); pcd = geometry::PointCloud({{0, 0, 0}, {1, 2, 3}}); aabb = pcd.GetAxisAlignedBoundingBox(); EXPECT_EQ(aabb.min_bound_, Eigen::Vector3d(0, 0, 0)); EXPECT_EQ(aabb.max_bound_, Eigen::Vector3d(1, 2, 3)); EXPECT_EQ(aabb.color_, Eigen::Vector3d(1, 1, 1)); } TEST(PointCloud, GetOrientedBoundingBox) { geometry::PointCloud pcd; geometry::OrientedBoundingBox obb; // Empty (GetOrientedBoundingBox requires >=4 points) pcd = geometry::PointCloud(); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); // Point pcd = geometry::PointCloud({{0, 0, 0}}); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetOrientedBoundingBox(true)); // Line pcd = geometry::PointCloud({{0, 0, 0}, {1, 1, 1}}); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); pcd = geometry::PointCloud({{0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetOrientedBoundingBox(true)); // Plane pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}}); EXPECT_ANY_THROW(pcd.GetOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetOrientedBoundingBox(true)); // Valid 4 points pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {1, 1, 1}}); pcd.GetOrientedBoundingBox(); // 8 points with known ground truth pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 2, 0}, {0, 2, 1}, {3, 0, 0}, {3, 0, 1}, {3, 2, 0}, {3, 2, 1}}); obb = pcd.GetOrientedBoundingBox(); EXPECT_EQ(obb.center_, Eigen::Vector3d(1.5, 1, 0.5)); EXPECT_EQ(obb.extent_, Eigen::Vector3d(3, 2, 1)); EXPECT_EQ(obb.color_, Eigen::Vector3d(1, 1, 1)); EXPECT_EQ(obb.R_, Eigen::Matrix3d::Identity()); ExpectEQ(Sort(obb.GetBoxPoints()), Sort(std::vector({{0, 0, 0}, {0, 0, 1}, {0, 2, 0}, {0, 2, 1}, {3, 0, 0}, {3, 0, 1}, {3, 2, 0}, {3, 2, 1}}))); // Check for a bug where the OBB rotation contained a reflection for this // example. pcd = geometry::PointCloud({{0, 2, 4}, {7, 9, 1}, {5, 2, 0}, {3, 8, 7}}); obb = pcd.GetOrientedBoundingBox(); EXPECT_GT(obb.R_.determinant(), 0.999); } TEST(PointCloud, GetMinimalOrientedBoundingBox) { geometry::PointCloud pcd; geometry::OrientedBoundingBox obb; // Empty (GetOrientedBoundingBox requires >=4 points) pcd = geometry::PointCloud(); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); // Point pcd = geometry::PointCloud({{0, 0, 0}}); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetMinimalOrientedBoundingBox(true)); // Line pcd = geometry::PointCloud({{0, 0, 0}, {1, 1, 1}}); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); pcd = geometry::PointCloud({{0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}}); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetMinimalOrientedBoundingBox(true)); // Plane pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}}); EXPECT_ANY_THROW(pcd.GetMinimalOrientedBoundingBox()); EXPECT_NO_THROW(pcd.GetMinimalOrientedBoundingBox(true)); // Valid 4 points pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {1, 1, 1}}); pcd.GetMinimalOrientedBoundingBox(); // 8 points with known ground truth pcd = geometry::PointCloud({{0, 0, 0}, {0, 0, 1}, {0, 2, 0}, {0, 2, 1}, {3, 0, 0}, {3, 0, 1}, {3, 2, 0}, {3, 2, 1}}); obb = pcd.GetMinimalOrientedBoundingBox(); EXPECT_EQ(obb.center_, Eigen::Vector3d(1.5, 1, 0.5)); EXPECT_EQ(obb.extent_, Eigen::Vector3d(3, 2, 1)); EXPECT_EQ(obb.color_, Eigen::Vector3d(1, 1, 1)); ExpectEQ(Sort(obb.GetBoxPoints()), Sort(std::vector({{0, 0, 0}, {0, 0, 1}, {0, 2, 0}, {0, 2, 1}, {3, 0, 0}, {3, 0, 1}, {3, 2, 0}, {3, 2, 1}}))); // Check for a bug where the OBB rotation contained a reflection for this // example. pcd = geometry::PointCloud({{0, 2, 4}, {7, 9, 1}, {5, 2, 0}, {3, 8, 7}}); obb = pcd.GetMinimalOrientedBoundingBox(); EXPECT_GT(obb.R_.determinant(), 0.999); // should always be equal/smaller than axis aligned- & oriented bounding box pcd = geometry::PointCloud({{0.866, 0.474, 0.659}, {0.943, 0.025, 0.789}, {0.386, 0.264, 0.691}, {0.938, 0.588, 0.496}, {0.221, 0.116, 0.257}, {0.744, 0.182, 0.052}, {0.019, 0.525, 0.699}, {0.722, 0.134, 0.668}}); geometry::OrientedBoundingBox mobb = pcd.GetMinimalOrientedBoundingBox(); ; obb = pcd.GetOrientedBoundingBox(); geometry::AxisAlignedBoundingBox aabb = pcd.GetAxisAlignedBoundingBox(); EXPECT_GT(obb.Volume(), mobb.Volume()); EXPECT_GT(aabb.Volume(), mobb.Volume()); } TEST(PointCloud, Transform) { std::vector points = { {0, 0, 0}, {1, 2, 4}, }; std::vector normals = { {4, 2, 1}, {0, 0, 0}, }; std::vector covariances = { Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), }; // clang-format off Eigen::Matrix4d transformation; transformation << 0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5; Eigen::Matrix3d gt_covariance_transformed; gt_covariance_transformed << 1.25, 4.25, 7.25, 4.25, 19.25, 34.25, 7.25, 34.25, 61.25; // clang-format on std::vector points_transformed = { {0.20000, 0.46666, 0.73333}, {0.11926, 0.41284, 0.70642}, }; std::vector normals_transformed = { {2, 16, 30}, {0, 0, 0}, }; std::vector covariances_transformed = { gt_covariance_transformed, gt_covariance_transformed, }; geometry::PointCloud pcd; pcd.points_ = points; pcd.normals_ = normals; pcd.covariances_ = covariances; pcd.Transform(transformation); ExpectEQ(pcd.points_, points_transformed, 1e-4); ExpectEQ(pcd.normals_, normals_transformed, 1e-4); ExpectEQ(pcd.covariances_, covariances_transformed, 1e-4); } TEST(PointCloud, Translate) { std::vector points = { {0, 1, 2}, {6, 7, 8}, }; Eigen::Vector3d translation(10, 20, 30); std::vector points_translated = { {10, 21, 32}, {16, 27, 38}, }; std::vector points_translated_non_relative = { {7, 17, 27}, {13, 23, 33}, }; // Relative: direct translation. geometry::PointCloud pcd; pcd.points_ = points; pcd.Translate(translation); ExpectEQ(pcd.points_, points_translated); // Non-relative: new center is the translation. pcd.points_ = points; pcd.Translate(translation, /*relative=*/false); ExpectEQ(pcd.points_, points_translated_non_relative); ExpectEQ(pcd.GetCenter(), translation); } TEST(PointCloud, Scale) { std::vector points = {{0, 1, 2}, {6, 7, 8}}; double scale = 10; geometry::PointCloud pcd; pcd.points_ = points; pcd.Scale(scale, Eigen::Vector3d(0, 0, 0)); ExpectEQ(pcd.points_, std::vector({{0, 10, 20}, {60, 70, 80}})); pcd.points_ = points; pcd.Scale(scale, Eigen::Vector3d(1, 1, 1)); ExpectEQ(pcd.points_, std::vector({{-9, 1, 11}, {51, 61, 71}})); } TEST(PointCloud, Rotate) { std::vector points = {{0, 1, 2}, {3, 4, 5}}; std::vector normals = {{5, 4, 3}, {2, 1, 0}}; std::vector covariances = {Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero()}; Eigen::Matrix3d R; R = Eigen::AngleAxisd(0.25 * M_PI, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.33 * M_PI, Eigen::Vector3d::UnitZ()); geometry::PointCloud pcd; pcd.points_ = points; pcd.normals_ = normals; pcd.covariances_ = covariances; Eigen::Vector3d center = pcd.GetCenter(); pcd.Rotate(R, center); std::vector points_rotated = { {0, 1.42016, 1.67409}, {3, 3.57984, 5.32591}, }; std::vector normals_transformed = { {3, 3.84816, 5.11778}, {0, 1.688476, 1.465963}, }; std::vector covariances_rotated = { Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), }; ExpectEQ(pcd.points_, points_rotated, 1e-4); ExpectEQ(pcd.normals_, normals_transformed, 1e-4); ExpectEQ(pcd.covariances_, covariances_rotated, 1e-4); // Rotate relative to the original center ExpectEQ(pcd.GetCenter(), center); } TEST(PointCloud, OperatorPlusEqual) { std::vector points_a = {{0, 1, 2}, {3, 4, 5}}; std::vector normals_a = {{0, 1, 2}, {3, 4, 5}}; std::vector colors_a = {{0, 1, 2}, {3, 4, 5}}; std::vector covariances_a = {Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero()}; std::vector points_b = {{6, 7, 8}, {9, 10, 11}}; std::vector normals_b = {{6, 7, 8}, {9, 10, 11}}; std::vector colors_b = {{6, 7, 8}, {9, 10, 11}}; std::vector covariances_b = {Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity()}; std::vector empty(0); std::vector empty_m(0); std::vector points_a_b = { {0, 1, 2}, {3, 4, 5}, {6, 7, 8}, {9, 10, 11}}; std::vector normals_a_b = { {0, 1, 2}, {3, 4, 5}, {6, 7, 8}, {9, 10, 11}}; std::vector colors_a_b = { {0, 1, 2}, {3, 4, 5}, {6, 7, 8}, {9, 10, 11}}; std::vector covariances_a_b = { Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity()}; geometry::PointCloud pc_a_full; geometry::PointCloud pc_b_full; pc_a_full.points_ = points_a; pc_a_full.normals_ = normals_a; pc_a_full.colors_ = colors_a; pc_a_full.covariances_ = covariances_a; pc_b_full.points_ = points_b; pc_b_full.normals_ = normals_b; pc_b_full.colors_ = colors_b; pc_b_full.covariances_ = covariances_b; geometry::PointCloud pc_a = pc_a_full; geometry::PointCloud pc_b = pc_b_full; pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, normals_a_b); ExpectEQ(pc_a.colors_, colors_a_b); ExpectEQ(pc_a.covariances_, covariances_a_b); pc_a = pc_a_full; pc_b = pc_b_full; pc_a.normals_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, empty); ExpectEQ(pc_a.colors_, colors_a_b); ExpectEQ(pc_a.covariances_, covariances_a_b); pc_a = pc_a_full; pc_b = pc_b_full; pc_b.normals_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, empty); ExpectEQ(pc_a.colors_, colors_a_b); ExpectEQ(pc_a.covariances_, covariances_a_b); pc_a = pc_a_full; pc_b = pc_b_full; pc_a.colors_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, normals_a_b); ExpectEQ(pc_a.colors_, empty); ExpectEQ(pc_a.covariances_, covariances_a_b); pc_a = pc_a_full; pc_b = pc_b_full; pc_b.colors_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, normals_a_b); ExpectEQ(pc_a.colors_, empty); ExpectEQ(pc_a.covariances_, covariances_a_b); pc_a = pc_a_full; pc_b = pc_b_full; pc_a.covariances_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, normals_a_b); ExpectEQ(pc_a.colors_, colors_a_b); ExpectEQ(pc_a.covariances_, empty_m); pc_a = pc_a_full; pc_b = pc_b_full; pc_b.covariances_.clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, points_a_b); ExpectEQ(pc_a.normals_, normals_a_b); ExpectEQ(pc_a.colors_, colors_a_b); ExpectEQ(pc_a.covariances_, empty_m); pc_a.Clear(); pc_a += pc_b_full; ExpectEQ(pc_a.points_, pc_b_full.points_); ExpectEQ(pc_a.normals_, pc_b_full.normals_); ExpectEQ(pc_a.colors_, pc_b_full.colors_); ExpectEQ(pc_a.covariances_, pc_b_full.covariances_); pc_a = pc_a_full; pc_b.Clear(); pc_a += pc_b; ExpectEQ(pc_a.points_, pc_a_full.points_); ExpectEQ(pc_a.normals_, pc_a_full.normals_); ExpectEQ(pc_a.colors_, pc_a_full.colors_); ExpectEQ(pc_a.covariances_, pc_a_full.covariances_); } TEST(PointCloud, OperatorPlus) { std::vector points_a = {{0, 1, 2}}; std::vector normals_a = {{0, 1, 2}}; std::vector colors_a = {{0, 1, 2}}; std::vector covariances_a = {Eigen::Matrix3d::Zero()}; std::vector points_b = {{3, 4, 5}}; std::vector normals_b = {{3, 4, 5}}; std::vector colors_b = {{3, 4, 5}}; std::vector covariances_b = {Eigen::Matrix3d::Identity()}; geometry::PointCloud pc_a; geometry::PointCloud pc_b; pc_a.points_ = points_a; pc_a.normals_ = normals_a; pc_a.colors_ = colors_a; pc_a.covariances_ = covariances_a; pc_b.points_ = points_b; pc_b.normals_ = normals_b; pc_b.colors_ = colors_b; pc_b.covariances_ = covariances_b; geometry::PointCloud pc_c = pc_a + pc_b; ExpectEQ(pc_c.points_, std::vector({{0, 1, 2}, {3, 4, 5}})); ExpectEQ(pc_c.normals_, std::vector({{0, 1, 2}, {3, 4, 5}})); ExpectEQ(pc_c.colors_, std::vector({{0, 1, 2}, {3, 4, 5}})); ExpectEQ(pc_c.covariances_, std::vector( {Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity()})); } TEST(PointCloud, HasPoints) { geometry::PointCloud pcd; EXPECT_FALSE(pcd.HasPoints()); pcd.points_.resize(5); EXPECT_TRUE(pcd.HasPoints()); } TEST(PointCloud, HasNormals) { geometry::PointCloud pcd; EXPECT_FALSE(pcd.HasNormals()); // False if #points == 0 pcd.points_.resize(0); pcd.normals_.resize(5); EXPECT_FALSE(pcd.HasNormals()); // False if not consistent pcd.points_.resize(4); pcd.normals_.resize(5); EXPECT_FALSE(pcd.HasNormals()); // True if non-zero and consistent pcd.points_.resize(5); pcd.normals_.resize(5); EXPECT_TRUE(pcd.HasNormals()); } TEST(PointCloud, HasColors) { geometry::PointCloud pcd; EXPECT_FALSE(pcd.HasNormals()); // False if #points == 0 pcd.points_.resize(0); pcd.colors_.resize(5); EXPECT_FALSE(pcd.HasColors()); // False if not consistent pcd.points_.resize(4); pcd.colors_.resize(5); EXPECT_FALSE(pcd.HasColors()); // True if non-zero and consistent pcd.points_.resize(5); pcd.colors_.resize(5); EXPECT_TRUE(pcd.HasColors()); } TEST(PointCloud, HasCovariances) { geometry::PointCloud pcd; EXPECT_FALSE(pcd.HasCovariances()); // False if #points == 0 pcd.points_.resize(0); pcd.covariances_.resize(5); EXPECT_FALSE(pcd.HasCovariances()); // False if not consistent pcd.points_.resize(4); pcd.covariances_.resize(5); EXPECT_FALSE(pcd.HasCovariances()); // True if non-zero and consistent pcd.points_.resize(5); pcd.covariances_.resize(5); EXPECT_TRUE(pcd.HasCovariances()); } TEST(PointCloud, NormalizeNormals) { geometry::PointCloud pcd; pcd.normals_ = {{2, 2, 2}, {1, 1, 1}, {-1, -1, -1}, {0, 0, 1}, {0, 1, 0}, {1, 0, 0}, {0, 0, 0}}; pcd.NormalizeNormals(); ExpectEQ(pcd.normals_, std::vector({ {0.57735, 0.57735, 0.57735}, {0.57735, 0.57735, 0.57735}, {-0.57735, -0.57735, -0.57735}, {0, 0, 1}, {0, 1, 0}, {1, 0, 0}, {0, 0, 0}, })); } TEST(PointCloud, PaintUniformColor) { geometry::PointCloud pcd; pcd.points_.resize(2); EXPECT_EQ(pcd.points_.size(), 2); EXPECT_EQ(pcd.colors_.size(), 0); Eigen::Vector3d color(0.125, 0.25, 0.5); pcd.PaintUniformColor(color); EXPECT_EQ(pcd.colors_.size(), 2); EXPECT_EQ(pcd.colors_, std::vector({color, color})); } TEST(PointCloud, SelectByIndex) { std::vector points({ {0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}, }); std::vector colors({ {0.0, 0.0, 0.0}, {0.1, 0.1, 0.1}, {0.2, 0.2, 0.2}, {0.3, 0.3, 0.3}, }); std::vector normals({ {10, 10, 10}, {11, 11, 11}, {12, 12, 12}, {13, 13, 13}, }); std::vector covariances({ 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Zero(), 3.0 * Eigen::Matrix3d::Identity(), }); std::vector indices{0, 1, 3}; geometry::PointCloud pcd; pcd.points_ = points; pcd.colors_ = colors; pcd.normals_ = normals; pcd.covariances_ = covariances; std::shared_ptr pc0 = pcd.SelectByIndex(indices); ExpectEQ(pc0->points_, std::vector({ {0, 0, 0}, {1, 1, 1}, {3, 3, 3}, })); ExpectEQ(pc0->colors_, std::vector({ {0.0, 0.0, 0.0}, {0.1, 0.1, 0.1}, {0.3, 0.3, 0.3}, })); ExpectEQ(pc0->normals_, std::vector({ {10, 10, 10}, {11, 11, 11}, {13, 13, 13}, })); ExpectEQ(pc0->covariances_, std::vector({ 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), })); std::shared_ptr pc1 = pcd.SelectByIndex(indices, /*invert=*/true); ExpectEQ(pc1->points_, std::vector({ {2, 2, 2}, })); ExpectEQ(pc1->colors_, std::vector({ {0.2, 0.2, 0.2}, })); ExpectEQ(pc1->normals_, std::vector({ {12, 12, 12}, })); ExpectEQ(pc1->covariances_, std::vector({ Eigen::Matrix3d::Zero(), })); } TEST(PointCloud, VoxelDownSample) { // voxel_size: 1 // points_min_bound: (0.5, 0.5, 0.5) // voxel_min_bound: (0, 0, 0) // points coordinates: range from [0.5, 2.5) // voxel_{i,j,k}: 0 <= i, j, k <= 2; 27 possible voxels std::vector points{ // voxel_{0, 0, 0} {0.5, 0.7, 0.6}, {0.6, 0.5, 0.7}, {0.7, 0.6, 0.5}, {0.8, 0.8, 0.8}, // voxel_{0, 1, 2} {0.5, 1.6, 2.4}, {0.6, 1.5, 2.3}, }; std::vector normals{ // voxel_{0, 0, 0} {0, 0, 1}, {0, 2, 3}, {0, 4, 5}, {0, 6, 7}, // voxel_{0, 1, 2} {1, 0, 1}, {1, 2, 3}, }; std::vector colors{ // voxel_{0, 0, 0} {0.0, 0.0, 0.1}, {0.0, 0.2, 0.3}, {0.0, 0.4, 0.5}, {0.0, 0.6, 0.7}, // voxel_{0, 1, 2} {0.1, 0.0, 0.1}, {0.1, 0.2, 0.3}, }; std::vector covariances{ // voxel_{0, 0, 0} Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), // voxel_{0, 1, 2} Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), }; geometry::PointCloud pcd; pcd.points_ = points; pcd.normals_ = normals; pcd.colors_ = colors; pcd.covariances_ = covariances; // Ground-truth reference std::vector points_down{ {0.65, 0.65, 0.65}, {0.55, 1.55, 2.35}, }; std::vector normals_down{ {0, 3, 4}, {1, 1, 2}, }; std::vector colors_down{ {0.0, 0.3, 0.4}, {0.1, 0.1, 0.2}, }; std::vector covariances_down{ Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Identity(), }; std::shared_ptr pc_down = pcd.VoxelDownSample(1.0); std::vector sort_indices = GetIndicesAToB(pc_down->points_, points_down); ExpectEQ(ApplyIndices(pc_down->points_, sort_indices), points_down); ExpectEQ(ApplyIndices(pc_down->normals_, sort_indices), normals_down); ExpectEQ(ApplyIndices(pc_down->colors_, sort_indices), colors_down); ExpectEQ(ApplyIndices(pc_down->covariances_, sort_indices), covariances_down); } TEST(PointCloud, UniformDownSample) { std::vector points({ {0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {4, 0, 0}, {5, 0, 0}, {6, 0, 0}, {7, 0, 0}, }); std::vector normals({ {0, 0, 0}, {0, 1, 0}, {0, 2, 0}, {0, 3, 0}, {0, 4, 0}, {0, 5, 0}, {0, 6, 0}, {0, 7, 0}, }); std::vector colors({ {0.0, 0.0, 0.0}, {0.0, 0.0, 0.1}, {0.0, 0.0, 0.2}, {0.0, 0.0, 0.3}, {0.0, 0.0, 0.4}, {0.0, 0.0, 0.5}, {0.0, 0.0, 0.6}, {0.0, 0.0, 0.7}, }); std::vector covariances({ 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), 4.0 * Eigen::Matrix3d::Identity(), 5.0 * Eigen::Matrix3d::Identity(), 6.0 * Eigen::Matrix3d::Identity(), 7.0 * Eigen::Matrix3d::Identity(), }); geometry::PointCloud pcd; pcd.points_ = points; pcd.normals_ = normals; pcd.colors_ = colors; pcd.covariances_ = covariances; std::shared_ptr pc_down = pcd.UniformDownSample(3); ExpectEQ(pc_down->points_, std::vector({ {0, 0, 0}, {3, 0, 0}, {6, 0, 0}, })); ExpectEQ(pc_down->normals_, std::vector({ {0, 0, 0}, {0, 3, 0}, {0, 6, 0}, })); ExpectEQ(pc_down->colors_, std::vector({ {0.0, 0.0, 0.0}, {0.0, 0.0, 0.3}, {0.0, 0.0, 0.6}, })); ExpectEQ(pc_down->covariances_, std::vector({ 0.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), 6.0 * Eigen::Matrix3d::Identity(), })); } TEST(PointCloud, FarthestPointDownSample) { geometry::PointCloud pcd({{0, 2.0, 0}, {1.0, 1.5, 0}, {0, 1.0, 0}, {1.0, 1.0, 0}, {0, 0, 1.0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}, {1.0, 1.0, 1.5}}); std::shared_ptr pcd_down = pcd.FarthestPointDownSample(4); ExpectEQ(pcd_down->points_, std::vector({{0, 2.0, 0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}})); } // namespace tests TEST(PointCloud, Crop_AxisAlignedBoundingBox) { geometry::AxisAlignedBoundingBox aabb({0, 0, 0}, {2, 2, 2}); geometry::PointCloud pcd({{0, 0, 0}, {2, 2, 2}, {1, 1, 1}, {1, 1, 2}, {3, 1, 1}, {-1, 1, 1}}); pcd.normals_ = {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {4, 0, 0}, {5, 0, 0}}; pcd.colors_ = {{0.0, 0.0, 0.0}, {0.1, 0.0, 0.0}, {0.2, 0.0, 0.0}, {0.3, 0.0, 0.0}, {0.4, 0.0, 0.0}, {0.5, 0.0, 0.0}}; pcd.covariances_ = { 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), 4.0 * Eigen::Matrix3d::Identity(), 5.0 * Eigen::Matrix3d::Identity(), }; std::shared_ptr pc_crop = pcd.Crop(aabb); ExpectEQ(pc_crop->points_, std::vector({ {0, 0, 0}, {2, 2, 2}, {1, 1, 1}, {1, 1, 2}, })); ExpectEQ(pc_crop->normals_, std::vector({ {0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, })); ExpectEQ(pc_crop->colors_, std::vector({ {0.0, 0.0, 0.0}, {0.1, 0.0, 0.0}, {0.2, 0.0, 0.0}, {0.3, 0.0, 0.0}, })); ExpectEQ(pc_crop->covariances_, std::vector({ 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), })); } TEST(PointCloud, Crop_OrientedBoundingBox) { geometry::OrientedBoundingBox obb(Eigen::Vector3d{1, 1, 1}, Eigen::Matrix3d::Identity(), Eigen::Vector3d{2, 2, 2}); geometry::PointCloud pcd({ {0, 0, 0}, {2, 2, 2}, {1, 1, 1}, {1, 1, 2}, {3, 1, 1}, {-1, 1, 1}, }); pcd.normals_ = {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {4, 0, 0}, {5, 0, 0}}; pcd.colors_ = {{0.0, 0.0, 0.0}, {0.1, 0.0, 0.0}, {0.2, 0.0, 0.0}, {0.3, 0.0, 0.0}, {0.4, 0.0, 0.0}, {0.5, 0.0, 0.0}}; pcd.covariances_ = { 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), 4.0 * Eigen::Matrix3d::Identity(), 5.0 * Eigen::Matrix3d::Identity(), }; std::shared_ptr pc_crop = pcd.Crop(obb); ExpectEQ(pc_crop->points_, std::vector({ {0, 0, 0}, {2, 2, 2}, {1, 1, 1}, {1, 1, 2}, })); ExpectEQ(pc_crop->normals_, std::vector({ {0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, })); ExpectEQ(pc_crop->colors_, std::vector({ {0.0, 0.0, 0.0}, {0.1, 0.0, 0.0}, {0.2, 0.0, 0.0}, {0.3, 0.0, 0.0}, })); ExpectEQ(pc_crop->covariances_, std::vector({ 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), })); } TEST(PointCloud, Crop_AxisAlignedBoundingBox_Invert) { geometry::AxisAlignedBoundingBox aabb({0, 0, 0}, {2, 2, 2}); geometry::PointCloud pcd({{0, 0, 0}, {2, 2, 2}, {1, 1, 1}, {1, 1, 2}, {3, 1, 1}, {-1, 1, 1}}); pcd.normals_ = {{0, 0, 0}, {1, 0, 0}, {2, 0, 0}, {3, 0, 0}, {4, 0, 0}, {5, 0, 0}}; pcd.colors_ = {{0.0, 0.0, 0.0}, {0.1, 0.0, 0.0}, {0.2, 0.0, 0.0}, {0.3, 0.0, 0.0}, {0.4, 0.0, 0.0}, {0.5, 0.0, 0.0}}; pcd.covariances_ = { 0.0 * Eigen::Matrix3d::Identity(), 1.0 * Eigen::Matrix3d::Identity(), 2.0 * Eigen::Matrix3d::Identity(), 3.0 * Eigen::Matrix3d::Identity(), 4.0 * Eigen::Matrix3d::Identity(), 5.0 * Eigen::Matrix3d::Identity(), }; std::shared_ptr pc_crop = pcd.Crop(aabb, true); ExpectEQ(pc_crop->points_, std::vector({{3, 1, 1}, {-1, 1, 1}})); ExpectEQ(pc_crop->normals_, std::vector({{4, 0, 0}, {5, 0, 0}})); ExpectEQ(pc_crop->colors_, std::vector({{0.4, 0.0, 0.0}, {0.5, 0.0, 0.0}})); ExpectEQ(pc_crop->covariances_, std::vector({4.0 * Eigen::Matrix3d::Identity(), 5.0 * Eigen::Matrix3d::Identity()})); } TEST(PointCloud, EstimateNormals) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}, }); pcd.EstimateNormals(geometry::KDTreeSearchParamKNN(/*knn=*/4)); pcd.NormalizeNormals(); double v = 1.0 / std::sqrt(3.0); ExpectEQ(pcd.normals_, std::vector({{v, v, v}, {-v, -v, v}, {v, -v, v}, {-v, v, v}, {-v, v, v}, {v, -v, v}, {-v, -v, v}, {v, v, v}})); } TEST(PointCloud, OrientNormalsToAlignWithDirection) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}, }); pcd.EstimateNormals(geometry::KDTreeSearchParamKNN(/*knn=*/4)); pcd.NormalizeNormals(); double v = 1.0 / std::sqrt(3.0); pcd.OrientNormalsToAlignWithDirection(Eigen::Vector3d{0, 0, -1}); ExpectEQ(pcd.normals_, std::vector({{-v, -v, -v}, {v, v, -v}, {-v, v, -v}, {v, -v, -v}, {v, -v, -v}, {-v, v, -v}, {v, v, -v}, {-v, -v, -v}})); // normal.norm() == 0 case pcd.points_ = std::vector{{10, 10, 10}}; pcd.normals_ = std::vector{{0, 0, 0}}; pcd.OrientNormalsToAlignWithDirection(Eigen::Vector3d{0, 0, -1}); ExpectEQ(pcd.normals_, std::vector{{0, 0, -1}}); } TEST(PointCloud, OrientNormalsTowardsCameraLocation) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 1, 0}, {1, 0, 0}, {1, 1, 0}, }); pcd.EstimateNormals(geometry::KDTreeSearchParamKNN(/*knn=*/4)); pcd.NormalizeNormals(); std::vector ref_normals( {{0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}}); std::vector ref_normals_rev( {{0, 0, -1}, {0, 0, -1}, {0, 0, -1}, {0, 0, -1}}); // Initial ExpectEQ(pcd.normals_, ref_normals); // Camera outside pcd.OrientNormalsTowardsCameraLocation(Eigen::Vector3d{2, 3, 4}); ExpectEQ(pcd.normals_, ref_normals); // Camera inside pcd.OrientNormalsTowardsCameraLocation(Eigen::Vector3d{-2, -3, -4}); ExpectEQ(pcd.normals_, ref_normals_rev); } TEST(PointCloud, OrientNormalsConsistentTangentPlane) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}, {0.5, 0.5, -0.25}, {0.5, 0.5, 1.25}, {0.5, -0.25, 0.5}, {0.5, 1.25, 0.5}, {-0.25, 0.5, 0.5}, {1.25, 0.5, 0.5}, }); // Hard-coded test pcd.EstimateNormals(geometry::KDTreeSearchParamKNN(/*knn=*/4)); double a = 0.57735; double b = 0.0927618; double c = 0.991358; ExpectEQ(pcd.normals_, std::vector({{a, a, a}, {-a, -a, a}, {a, -a, a}, {-a, a, a}, {-a, a, a}, {a, -a, a}, {-a, -a, a}, {a, a, a}, {-b, -b, -c}, {b, b, -c}, {b, c, b}, {-b, c, -b}, {c, b, b}, {c, -b, -b}})); pcd.OrientNormalsConsistentTangentPlane(/*k=*/4); ExpectEQ(pcd.normals_, std::vector({{-a, -a, -a}, {-a, -a, a}, {-a, a, -a}, {-a, a, a}, {a, -a, -a}, {a, -a, a}, {a, a, -a}, {a, a, a}, {-b, -b, -c}, {-b, -b, c}, {-b, -c, -b}, {-b, c, -b}, {-c, -b, -b}, {c, -b, -b}})); } TEST(PointCloud, ComputePointCloudToPointCloudDistance) { geometry::PointCloud pc0({{0, 0, 0}, {1, 2, 0}, {2, 2, 0}}); geometry::PointCloud pc1({{-1, 0, 0}, {-2, 0, 0}, {-1, 2, 0}}); pc0.ComputePointCloudDistance(pc1); ExpectEQ(pc0.ComputePointCloudDistance(pc1), std::vector({1, 2, 3})); } TEST(PointCloud, RemoveDuplicatedPoints) { geometry::PointCloud pc({{0, 0, 0}, {1, 3, 0}, {1, 3, 0}, {2, 2, 0}}); std::vector normals = {{1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}, {7.0, 8.0, 9.0}, {10.0, 11.0, 12.0}}; std::vector colors = { {0.5, 0.5, 0.5}, {0.6, 0.6, 0.6}, {0.7, 0.7, 0.7}, {0.8, 0.8, 0.8}}; pc.normals_ = normals; pc.colors_ = colors; std::vector ref_points = {{0, 0, 0}, {1, 3, 0}, {2, 2, 0}}; std::vector ref_normals = { {1.0, 2.0, 3.0}, {4.0, 5.0, 6.0}, {10.0, 11.0, 12.0}}; std::vector ref_colors = { {0.5, 0.5, 0.5}, {0.6, 0.6, 0.6}, {0.8, 0.8, 0.8}}; pc.RemoveDuplicatedPoints(); ExpectEQ(ref_points, pc.points_); ExpectEQ(ref_normals, pc.normals_); ExpectEQ(ref_colors, pc.colors_); } // TODO(Nacho): Add covariances unit tests TEST(PointCloud, DISABLED_EstimatePerPointCovariances) { NotImplemented(); } TEST(PointCloud, DISABLED_EstimateCovariances) { NotImplemented(); } TEST(PointCloud, ComputeMeanAndCovariance) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}, }); Eigen::Vector3d ref_mean(0.5, 0.5, 0.5); Eigen::Matrix3d ref_covariance; ref_covariance << 0.25, 0, 0, 0, 0.25, 0, 0, 0, 0.25; Eigen::Vector3d mean; Eigen::Matrix3d covariance; std::tie(mean, covariance) = pcd.ComputeMeanAndCovariance(); ExpectEQ(mean, ref_mean); ExpectEQ(covariance, ref_covariance); pcd.points_ = {}; ref_mean = Eigen::Vector3d::Zero(); ref_covariance = Eigen::Matrix3d::Identity(); std::tie(mean, covariance) = pcd.ComputeMeanAndCovariance(); ExpectEQ(mean, ref_mean); ExpectEQ(covariance, ref_covariance); pcd.points_ = {{1, 1, 1}}; ref_mean = Eigen::Vector3d({1, 1, 1}); ref_covariance = Eigen::Matrix3d::Zero(); std::tie(mean, covariance) = pcd.ComputeMeanAndCovariance(); ExpectEQ(mean, ref_mean); ExpectEQ(covariance, ref_covariance); } TEST(PointCloud, ComputeMahalanobisDistance) { geometry::PointCloud pcd({ {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 0, 2}, {1, 1, 1}, }); std::vector distance = pcd.ComputeMahalanobisDistance(); ExpectEQ(distance, std::vector({1.77951, 0.81650, 2.00000, 1.77951, 2.00000}), 1e-4); // Empty pcd.points_ = {}; distance = pcd.ComputeMahalanobisDistance(); ExpectEQ(distance, std::vector({})); // Nan if the covariance is not inversable pcd.points_ = {{1, 1, 1}}; distance = pcd.ComputeMahalanobisDistance(); EXPECT_EQ(distance.size(), 1); EXPECT_TRUE(std::isnan(distance[0])); pcd.points_ = {{0, 0, 0}, {1, 1, 1}}; distance = pcd.ComputeMahalanobisDistance(); EXPECT_EQ(distance.size(), 2); EXPECT_TRUE(std::isnan(distance[0]) && std::isnan(distance[1])); } TEST(PointCloud, ComputeNearestNeighborDistance) { geometry::PointCloud pcd; // Regular case pcd.points_ = {{0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {1, 2, 0}}; ExpectEQ(pcd.ComputeNearestNeighborDistance(), std::vector({0, 0, 1, 2})); // < 2 points pcd.points_ = {}; ExpectEQ(pcd.ComputeNearestNeighborDistance(), std::vector({})); pcd.points_ = {{10, 10, 10}}; ExpectEQ(pcd.ComputeNearestNeighborDistance(), std::vector({0})); // 2 points pcd.points_ = {{0, 0, 0}, {1, 0, 0}}; ExpectEQ(pcd.ComputeNearestNeighborDistance(), std::vector({1, 1})); } TEST(PointCloud, ComputeConvexHull) { geometry::PointCloud pcd; std::shared_ptr mesh; std::vector pt_map; // Needs at least 4 points pcd.points_ = {}; EXPECT_ANY_THROW(pcd.ComputeConvexHull()); pcd.points_ = {{0, 0, 0}}; EXPECT_ANY_THROW(pcd.ComputeConvexHull()); pcd.points_ = {{0, 0, 0}, {0, 0, 1}}; EXPECT_ANY_THROW(pcd.ComputeConvexHull()); pcd.points_ = {{0, 0, 0}, {0, 0, 1}, {0, 1, 0}}; EXPECT_ANY_THROW(pcd.ComputeConvexHull()); // Degenerate input pcd.points_ = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; EXPECT_ANY_THROW(pcd.ComputeConvexHull()); // Allow adding random noise to fix the degenerate input EXPECT_NO_THROW(pcd.ComputeConvexHull(true)); // Hard-coded test pcd.points_ = {{0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {1, 0, 0}}; std::tie(mesh, pt_map) = pcd.ComputeConvexHull(); EXPECT_EQ(pt_map, std::vector({2, 3, 0, 1})); ExpectEQ(mesh->vertices_, ApplyIndices(pcd.points_, pt_map)); // Hard-coded test pcd.points_ = {{0.5, 0.5, 0.5}, {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1}}; std::tie(mesh, pt_map) = pcd.ComputeConvexHull(); EXPECT_EQ(pt_map, std::vector({7, 3, 1, 5, 6, 2, 8, 4})); ExpectEQ(mesh->vertices_, ApplyIndices(pcd.points_, pt_map)); ExpectEQ(mesh->triangles_, std::vector({{1, 0, 2}, {0, 3, 2}, {3, 4, 2}, {4, 5, 2}, {0, 4, 3}, {4, 0, 6}, {7, 1, 2}, {5, 7, 2}, {7, 0, 1}, {0, 7, 6}, {4, 7, 5}, {7, 4, 6}})); } TEST(PointCloud, HiddenPointRemoval) { geometry::PointCloud pcd; data::PLYPointCloud pointcloud_ply; io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); EXPECT_EQ(pcd.points_.size(), 196133); ExpectEQ(pcd.GetMaxBound(), Eigen::Vector3d(3.96609, 2.427476, 2.55859)); ExpectEQ(pcd.GetMinBound(), Eigen::Vector3d(0.558594, 0.832031, 0.566637)); // Hard-coded test std::shared_ptr mesh; std::vector pt_map; std::tie(mesh, pt_map) = pcd.HiddenPointRemoval(Eigen::Vector3d{0, 0, 5}, 5 * 100); ExpectEQ(mesh->vertices_, ApplyIndices(pcd.points_, pt_map)); EXPECT_EQ(mesh->vertices_.size(), 24581); } TEST(PointCloud, ClusterDBSCAN) { geometry::PointCloud pcd; data::PLYPointCloud pointcloud_ply; io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); EXPECT_EQ(pcd.points_.size(), 196133); // Hard-coded test std::vector cluster = pcd.ClusterDBSCAN(0.02, 10, false); EXPECT_EQ(cluster.size(), 196133); std::unordered_set cluster_set(cluster.begin(), cluster.end()); EXPECT_EQ(cluster_set.size(), 11); int cluster_sum = std::accumulate(cluster.begin(), cluster.end(), 0); EXPECT_EQ(cluster_sum, 398580); } TEST(PointCloud, SegmentPlane) { geometry::PointCloud pcd; data::PCDPointCloud pointcloud_pcd; io::ReadPointCloud(pointcloud_pcd.GetPath(), pcd); EXPECT_EQ(pcd.points_.size(), 113662); // Hard-coded test Eigen::Vector4d plane_model; std::vector inliers; std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 3, 1000); ExpectEQ(plane_model, Eigen::Vector4d(-0.06, -0.10, 0.99, -1.06), 0.1); std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 10, 1000); ExpectEQ(plane_model, Eigen::Vector4d(-0.06, -0.10, 0.99, -1.06), 0.1); } TEST(PointCloud, SegmentPlaneKnownPlane) { // Points sampled from the plane x + y + z + 1 = 0 std::vector ref = {{2.0, 1.0, -4.0}, {1.0, 3.0, -5.0}, {-2.0, -1.0, 2.0}, {-2.0, -2.0, 3.0}, {10.0, 10.0, -21.0}}; geometry::PointCloud pcd(ref); Eigen::Vector4d plane_model; std::vector inliers; std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 3, 10); ExpectEQ(pcd.SelectByIndex(inliers)->points_, ref); std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 4, 10); ExpectEQ(pcd.SelectByIndex(inliers)->points_, ref); } TEST(PointCloud, SegmentPlaneSpecialCase) { // Test SegmentPlane with probability == 1, <= 0 and > 1. // Points sampled from the plane x + y + z + 1 = 0 std::vector ref = {{2.0, 1.0, -4.0}, {1.0, 3.0, -5.0}, {-2.0, -1.0, 2.0}, {-2.0, -2.0, 3.0}, {10.0, 10.0, -21.0}}; geometry::PointCloud pcd(ref); Eigen::Vector4d plane_model; std::vector inliers; std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 3, 10, 1); ExpectEQ(pcd.SelectByIndex(inliers)->points_, ref); EXPECT_ANY_THROW(pcd.SegmentPlane(0.01, 3, 10, 0)); EXPECT_ANY_THROW(pcd.SegmentPlane(0.01, 3, 10, -1)); EXPECT_ANY_THROW(pcd.SegmentPlane(0.01, 3, 10, 1.5)); } TEST(PointCloud, SegmentPlaneDeterministic) { geometry::PointCloud pcd; data::PCDPointCloud pointcloud_pcd; io::ReadPointCloud(pointcloud_pcd.GetPath(), pcd); EXPECT_EQ(pcd.points_.size(), 113662); // Hard-coded test Eigen::Vector4d plane_model; std::vector inliers; utility::random::Seed(0); std::tie(plane_model, inliers) = pcd.SegmentPlane(0.01, 3, 1000, 1.0); ExpectEQ(plane_model, Eigen::Vector4d(-0.06, -0.10, 0.99, -1.06), 0.1); // Test segment plane for 10 times with the same random seed. for (int i = 0; i < 10; ++i) { // Reset random seed. utility::random::Seed(0); Eigen::Vector4d plane_model_d; std::vector inliers_d; std::tie(plane_model_d, inliers_d) = pcd.SegmentPlane(0.01, 3, 1000, 1.0); ExpectEQ(plane_model, plane_model_d); } } TEST(PointCloud, DetectPlanarPatches) { geometry::PointCloud pcd; data::PCDPointCloud pointcloud_pcd; io::ReadPointCloud(pointcloud_pcd.GetPath(), pcd); EXPECT_EQ(pcd.points_.size(), 113662); static constexpr int nrNeighbors = 75; const geometry::KDTreeSearchParam& search_param = geometry::KDTreeSearchParamKNN(nrNeighbors); pcd.EstimateNormals(search_param); // set parameters constexpr double normal_variance_threshold_deg = 60; constexpr double coplanarity_deg = 89; constexpr double outlier_ratio = 0.25; constexpr double min_plane_edge_length = 0.0; constexpr size_t min_num_points = 30; std::vector> patches; patches = pcd.DetectPlanarPatches( normal_variance_threshold_deg, coplanarity_deg, outlier_ratio, min_plane_edge_length, min_num_points, search_param); EXPECT_EQ(patches.size(), 6); double largest_area = 0; std::shared_ptr largest_patch; for (const auto& obox : patches) { const double area = obox->extent_.x() * obox->extent_.y(); if (area > largest_area) { largest_patch = obox; largest_area = area; } } const Eigen::Vector3d n = largest_patch->R_.col(2); const double d = -n.dot(largest_patch->center_); Eigen::Vector4d plane_model = Eigen::Vector4d(n.x(), n.y(), n.z(), d); ExpectEQ(plane_model, Eigen::Vector4d(0.06, 0.10, -0.99, 1.06), 0.1); } TEST(PointCloud, CreateFromDepthImage) { data::SampleRedwoodRGBDImages redwood_data; const std::string trajectory_path = redwood_data.GetTrajectoryLogPath(); const std::string im_depth_path = redwood_data.GetDepthPaths()[0]; camera::PinholeCameraTrajectory trajectory; io::ReadPinholeCameraTrajectory(trajectory_path, trajectory); camera::PinholeCameraIntrinsic intrinsic = trajectory.parameters_[0].intrinsic_; Eigen::Matrix4d extrinsic = trajectory.parameters_[0].extrinsic_; std::shared_ptr im_depth = io::CreateImageFromFile(im_depth_path); std::shared_ptr pcd = geometry::PointCloud::CreateFromDepthImage(*im_depth, intrinsic, extrinsic); // Hard-coded test EXPECT_EQ(pcd->points_.size(), 267129); ExpectEQ(pcd->GetMinBound(), Eigen::Vector3d(-2.59579, 0.120689, 1.64421), 1e-5); ExpectEQ(pcd->GetMaxBound(), Eigen::Vector3d(-1.08349, 1.68228, 4.18797), 1e-5); EXPECT_EQ(pcd->colors_.size(), 0); // visualization::DrawGeometries({pcd}); // Uncomment for manual check } TEST(PointCloud, CreateFromRGBDImage) { data::SampleRedwoodRGBDImages redwood_data; const std::string trajectory_path = redwood_data.GetTrajectoryLogPath(); const std::string im_depth_path = redwood_data.GetDepthPaths()[0]; const std::string im_rgb_path = redwood_data.GetColorPaths()[0]; camera::PinholeCameraTrajectory trajectory; io::ReadPinholeCameraTrajectory(trajectory_path, trajectory); camera::PinholeCameraIntrinsic intrinsic = trajectory.parameters_[0].intrinsic_; Eigen::Matrix4d extrinsic = trajectory.parameters_[0].extrinsic_; std::shared_ptr im_depth = io::CreateImageFromFile(im_depth_path); std::shared_ptr im_depth_float = im_depth->ConvertDepthToFloatImage(); std::shared_ptr im_rgb = io::CreateImageFromFile(im_rgb_path); geometry::RGBDImage im_rgbd(*im_rgb, *im_depth_float); std::shared_ptr pcd = geometry::PointCloud::CreateFromRGBDImage(im_rgbd, intrinsic, extrinsic); // Hard-coded test EXPECT_EQ(pcd->points_.size(), 267129); ExpectEQ(pcd->GetMinBound(), Eigen::Vector3d(-2.59579, 0.120689, 1.64421), 1e-5); ExpectEQ(pcd->GetMaxBound(), Eigen::Vector3d(-1.08349, 1.68228, 4.18797), 1e-5); EXPECT_EQ(pcd->colors_.size(), pcd->points_.size()); // visualization::DrawGeometries({pcd}); // Uncomment for manual check } } // namespace tests } // namespace open3d