From d6b9e9d3fc1ca2fedaca03cd5736025cbfd9d23d Mon Sep 17 00:00:00 2001 From: Yoshua Nava Date: Mon, 6 Mar 2023 22:49:28 +0100 Subject: [PATCH 1/9] Implemented a class to generate point clouds of 3D geometric primitives --- CMakeLists.txt | 4 +- pointmatcher/PointCloudGenerator.cpp | 360 +++++++++++++++++++++++++++ pointmatcher/PointMatcher.h | 98 ++++++++ utest/CMakeLists.txt | 1 + utest/ui/PointCloudGenerator.cpp | 233 +++++++++++++++++ 5 files changed, 695 insertions(+), 1 deletion(-) create mode 100644 pointmatcher/PointCloudGenerator.cpp create mode 100644 utest/ui/PointCloudGenerator.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d78484a8..534d79ec8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -355,7 +355,9 @@ set(POINTMATCHER_SRC pointmatcher/DataPointsFilters/RemoveSensorBias.cpp pointmatcher/DataPointsFilters/Sphericality.cpp pointmatcher/DataPointsFilters/Saliency.cpp - pointmatcher/DataPointsFilters/SpectralDecomposition.cpp + pointmatcher/DataPointsFilters/SpectralDecomposition.cpp +#PointCloudGenerators + pointmatcher/PointCloudGenerator ) diff --git a/pointmatcher/PointCloudGenerator.cpp b/pointmatcher/PointCloudGenerator.cpp new file mode 100644 index 000000000..b8f6dce85 --- /dev/null +++ b/pointmatcher/PointCloudGenerator.cpp @@ -0,0 +1,360 @@ +// kate: replace-tabs off; indent-width 4; indent-mode normal +// vim: ts=4:sw=4:noexpandtab +/* + +Copyright (c) 2023, +Yoshua Nava, ANYbotics AG, Switzerland +You can contact the authors at and + + +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#include "PointMatcher.h" + +#include +#include + +template +typename PointMatcher::AffineTransform PointMatcher::PointCloudGenerator::buildUpTransformation(const StaticCoordVector& translation, + const Quaternion& rotation) +{ + AffineTransform transformation; + transformation.translation() = translation; + transformation.linear() = rotation.normalized().toRotationMatrix(); + return transformation; +} + +template +void PointMatcher::PointCloudGenerator::addEmpty3dPointFields(const Index numberOfPoints, DataPoints& pointCloud) +{ + using Label = typename DataPoints::Label; + + // Add features. + pointCloud.features = Matrix::Ones(4, numberOfPoints); + + // Add feature labels. + pointCloud.featureLabels.push_back(Label("x", 1)); + pointCloud.featureLabels.push_back(Label("y", 1)); + pointCloud.featureLabels.push_back(Label("z", 1)); + pointCloud.featureLabels.push_back(Label("pad", 1)); + + // Add descriptors. + pointCloud.addDescriptor("normals", Matrix::Constant(3, numberOfPoints, 0)); +} + +template +void PointMatcher::PointCloudGenerator::applyTransformation(const StaticCoordVector& translation, const Quaternion& rotation, + DataPoints& pointCloud) +{ + // Convert (translation, rotation) into a representation + const AffineTransform transformation{ buildUpTransformation(translation, rotation) }; + + // Transformation handler; + std::shared_ptr transformator(PointMatcher::get().REG(Transformation).create("RigidTransformation")); + + // Apply transformation. + transformator->inPlaceCompute(transformation.matrix(), pointCloud); +} + +template +typename PointMatcher::StaticCoordVector PointMatcher::PointCloudGenerator::computeNormalOfAxisAlignedPlane( + const StaticCoordVector& axisAlignedPlaneDimensions) +{ + StaticCoordVector normalVector{ StaticCoordVector::Zero() }; + for (Index i{ 0 }; i < 3; ++i) + { + if (axisAlignedPlaneDimensions(i) == 0) + { + normalVector(i) = 1; + } + } + return normalVector; +} + +// Reference: http://corysimon.github.io/articles/uniformdistn-on-sphere/ +template +typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::generateUniformlySampledSphere( + const ScalarType radius, + const Index numberOfPoints, + const StaticCoordVector& translation, + const Quaternion& rotation) +{ + // Create point cloud and add basic structure to fill. + DataPoints pointCloud; + addEmpty3dPointFields(numberOfPoints, pointCloud); + + // Create random distribution generators. + std::random_device randomDevice; + std::mt19937 randomNumberGenerator(randomDevice()); + std::uniform_real_distribution uniformDistribution(0.0f, 1.0f); + + // Sampling in Spherical coordinates. + auto normalsView{ pointCloud.getDescriptorViewByName("normals") }; + for (Index i{ 0 }; i < numberOfPoints; ++i) + { + // Sample random values of theta and phi. + const ScalarType theta{ 2.0f * pi * uniformDistribution(randomNumberGenerator) }; + const ScalarType phi{ std::acos(1.0f - 2.0f * uniformDistribution(randomNumberGenerator)) }; + + // Pre-compute values, such as sine and cosine of phi and theta. + const ScalarType sinPhi{ std::sin(phi) }; + const ScalarType cosPhi{ std::cos(phi) }; + const ScalarType sinTheta{ std::sin(theta) }; + const ScalarType cosTheta{ std::cos(theta) }; + + // Fill features (3D point coordinates) + pointCloud.features(0, i) = sinPhi * cosTheta * radius; // x + pointCloud.features(1, i) = sinPhi * sinTheta * radius; // y + pointCloud.features(2, i) = cosPhi * radius; // z + + // For a sphere, it's possible to compute outward looking normals by simply normalizing the vector going from its center to the + // form's body. + const StaticCoordVector normalVector{ pointCloud.features.col(i).head(3).normalized() }; + + // Fill normals. + normalsView(0, i) = normalVector(0); + normalsView(1, i) = normalVector(1); + normalsView(2, i) = normalVector(2); + } + + // Transform point cloud in space. + applyTransformation(translation, rotation, pointCloud); + + return pointCloud; +} + +// Reference: https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly +template +typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::generateUniformlySampledCircle( + const ScalarType radius, + const Index numberOfPoints, + const StaticCoordVector& translation, + const Quaternion& rotation) +{ + // Create point cloud and add basic structure to fill. + DataPoints pointCloud; + addEmpty3dPointFields(numberOfPoints, pointCloud); + + // Create random distribution generators. + std::random_device randomDevice; + std::mt19937 randomNumberGenerator(randomDevice()); + std::uniform_real_distribution uniformDistribution(0.0f, 1.0f); + + // Sampling in Cartesian coordinates. + auto normalsView{ pointCloud.getDescriptorViewByName("normals") }; + for (Index i{ 0 }; i < numberOfPoints; ++i) + { + // Sample random values of theta and phi. + const ScalarType phi{ 2.0f * pi * uniformDistribution(randomNumberGenerator) }; + const ScalarType radiusSample{ radius * uniformDistribution(randomNumberGenerator) }; + + // Pre-compute values, such as sine and cosine of phi and theta. + const ScalarType sinPhi{ std::sin(phi) }; + const ScalarType cosPhi{ std::cos(phi) }; + + // Fill features (3D point coordinates) + pointCloud.features(0, i) = cosPhi * radiusSample; // x + pointCloud.features(1, i) = sinPhi * radiusSample; // y + pointCloud.features(2, i) = 0; // z + + // Fill normals. + normalsView(0, i) = 0; + normalsView(1, i) = 0; + normalsView(2, i) = 1; + } + + // We generated the circle on the ground plane, and now we rotate it based on the translation and rotation arguments of this function. + applyTransformation(translation, rotation, pointCloud); + + return pointCloud; +} + +// Reference: https://stackoverflow.com/questions/2678501/uniform-generation-of-3d-points-on-cylinder-cone +template +typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::generateUniformlySampledCylinder( + const ScalarType radius, const ScalarType height, const Index numberOfPoints, const StaticCoordVector& translation, + const Quaternion& rotation) +{ + // Create point cloud. + DataPoints pointCloud; + + // Define number of points per section of the volume. + // Cylinder area S = 2 * pi * r * h + 2 * pi * r^2 + // where r: radius + // h: height + // The ratio between the caps and the body is given by the ratio between the area of the caps and the total area of the solid. + // R = (2 * pi * r^2) / (2 * pi * r * h + 2 * pi * r^2) = r / (h + r) + const ScalarType ratioCaps{ radius / (height + radius) }; + // Round to nearest even number + const Index numberOfPointsBothCaps{ static_cast(std::round(static_cast(numberOfPoints) * ratioCaps * 0.5f)) * 2u }; + // Split between the two caps + const Index numberOfPointsCap{ numberOfPointsBothCaps / 2u }; + // Put the rest of the points in the body + const Index numberOfPointsBody{ numberOfPoints - numberOfPointsBothCaps }; + + // Create random distribution generators. + std::random_device randomDevice; + std::mt19937 randomNumberGenerator(randomDevice()); + std::uniform_real_distribution uniformDistribution(-1.0f, 1.0f); + + addEmpty3dPointFields(numberOfPointsBody, pointCloud); + + // Sampling in Cylindrical coordinates. + // This loop builds the Cylinder body. + auto normalsView{ pointCloud.getDescriptorViewByName("normals") }; + for (Index i{ 0 }; i < numberOfPointsBody; ++i) + { + // Sample random values of theta and phi. + const ScalarType phi{ 2.0f * pi * uniformDistribution(randomNumberGenerator) }; + const ScalarType z{ height * 0.5f * uniformDistribution(randomNumberGenerator) }; + + // Pre-compute values, such as sine and cosine of phi and theta. + const ScalarType sinPhi{ std::sin(phi) }; + const ScalarType cosPhi{ std::cos(phi) }; + + // Fill features (3D point coordinates) + pointCloud.features(0, i) = cosPhi * radius; // x + pointCloud.features(1, i) = sinPhi * radius; // y + pointCloud.features(2, i) = z; // z + + // For a cylinder, it's possible to compute outward looking normals by simply normalizing the vector going from its center to the + // cylinder's body. + const StaticCoordVector radiusVector(pointCloud.features(0, i), pointCloud.features(1, i), 0); + const StaticCoordVector normalVector{ radiusVector.normalized() }; + + // Fill normals. + normalsView(0, i) = normalVector(0); + normalsView(1, i) = normalVector(1); + normalsView(2, i) = normalVector(2); + } + + // Add top and bottom caps. + const Quaternion topCapOrientation{ Quaternion::Identity() }; + const Quaternion bottomCapOrientation{ 0, 1, 0, 0 }; // Flip 180 degrees around x. + const StaticCoordVector topCapTranslation{ 0.0f, 0.0f, height * 0.5f }; + const StaticCoordVector bottomCapTranslation{ 0.0f, 0.0f, -height * 0.5f }; + pointCloud.concatenate(generateUniformlySampledCircle(radius, numberOfPointsCap, topCapTranslation, topCapOrientation)); + pointCloud.concatenate(generateUniformlySampledCircle(radius, numberOfPointsCap, bottomCapTranslation, bottomCapOrientation)); + + // Transform point cloud in space. + applyTransformation(translation, rotation, pointCloud); + + return pointCloud; +} + +// Reference: https://stackoverflow.com/questions/11815792/generation-of-3d-random-points-on-the-surface-of-a-cube +template +typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::generateUniformlySampledPlane( + const StaticCoordVector& dimensions, + const Index numberOfPoints, + const StaticCoordVector& translation, + const Quaternion& rotation) +{ + // Create point cloud and add basic structure to fill. + DataPoints pointCloud; + addEmpty3dPointFields(numberOfPoints, pointCloud); + + // Create random distribution generators. + std::random_device randomDevice; + std::mt19937 randomNumberGenerator(randomDevice()); + std::uniform_real_distribution lengthUniformDistribution(-dimensions(0), dimensions(0)); + std::uniform_real_distribution widthUniformDistribution(-dimensions(1), dimensions(1)); + std::uniform_real_distribution heightUniformDistribution(-dimensions(2), dimensions(2)); + + const StaticCoordVector normalVector{ computeNormalOfAxisAlignedPlane(dimensions) }; + + // Sampling in Cartesian coordinates. + auto normalsView{ pointCloud.getDescriptorViewByName("normals") }; + for (Index i{ 0 }; i < numberOfPoints; ++i) + { + // Fill features (3D point coordinates) + pointCloud.features(0, i) = lengthUniformDistribution(randomNumberGenerator); // x + pointCloud.features(1, i) = widthUniformDistribution(randomNumberGenerator); // y + pointCloud.features(2, i) = heightUniformDistribution(randomNumberGenerator); // z + + // Fill normals. + normalsView(0, i) = normalVector(0); + normalsView(1, i) = normalVector(1); + normalsView(2, i) = normalVector(2); + } + + // We generated the plane on the ground plane, and now we rotate it based on the translation and rotation arguments of this function. + applyTransformation(translation, rotation, pointCloud); + + return pointCloud; +} + +template +typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::generateUniformlySampledBox( + const ScalarType length, const ScalarType width, const ScalarType height, const Index numberOfPoints, + const StaticCoordVector& translation, const Quaternion& rotation) +{ + const Index numberOfFaces{ 6 }; + const Index numberOfPointsPerFace{ numberOfPoints / numberOfFaces }; + + // Create point cloud and add basic structure to fill. + DataPoints pointCloud; + addEmpty3dPointFields(0, pointCloud); + + // Unit vectors for vertices. + // TODO(ynava) Evaluate generating a unit-length box and then re-scaling it. + const StaticCoordVector positiveXaxisFaceCenter{ length * 0.5f, 0.0f, 0.0f }; + const StaticCoordVector negativeXaxisFaceCenter{ -length * 0.5f, 0.0f, 0.0f }; + const StaticCoordVector positiveYaxisFaceCenter{ 0.0f, width * 0.5f, 0.0f }; + const StaticCoordVector negativeYaxisFaceCenter{ 0.0f, -width * 0.5f, 0.0f }; + const StaticCoordVector positiveZaxisFaceCenter{ 0.0f, 0.0f, height * 0.5f }; + const StaticCoordVector negativeZaxisFaceCenter{ 0.0f, 0.0f, -height * 0.5f }; + + // Dimension vectors of each of the box faces. + const Quaternion faceOrientation{ Quaternion::Identity() }; + const StaticCoordVector xyFaceDimensions{ length * 0.5f, width * 0.5f, 0.0f }; + const StaticCoordVector yzFaceDimensions{ 0.0f, width * 0.5f, height * 0.5f }; + const StaticCoordVector xzFaceDimensions{ length * 0.5f, 0.0f, height * 0.5f }; + + // Add points from each face to the point cloud. + pointCloud.concatenate( + generateUniformlySampledPlane(yzFaceDimensions, numberOfPointsPerFace, positiveXaxisFaceCenter, faceOrientation)); + pointCloud.concatenate( + generateUniformlySampledPlane(yzFaceDimensions, numberOfPointsPerFace, negativeXaxisFaceCenter, faceOrientation)); + pointCloud.concatenate( + generateUniformlySampledPlane(xzFaceDimensions, numberOfPointsPerFace, positiveYaxisFaceCenter, faceOrientation)); + pointCloud.concatenate( + generateUniformlySampledPlane(xzFaceDimensions, numberOfPointsPerFace, negativeYaxisFaceCenter, faceOrientation)); + pointCloud.concatenate( + generateUniformlySampledPlane(xyFaceDimensions, numberOfPointsPerFace, positiveZaxisFaceCenter, faceOrientation)); + const Index missingPointsLastFace{ numberOfPoints - pointCloud.getNbPoints() - numberOfPointsPerFace }; + pointCloud.concatenate(generateUniformlySampledPlane( + xyFaceDimensions, numberOfPointsPerFace + missingPointsLastFace, negativeZaxisFaceCenter, faceOrientation)); + + // Transform point cloud in space. + applyTransformation(translation, rotation, pointCloud); + + return pointCloud; +} + +template struct PointMatcher::PointCloudGenerator; +template struct PointMatcher::PointCloudGenerator; diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h index f26aa2ce0..65ea54e47 100644 --- a/pointmatcher/PointMatcher.h +++ b/pointmatcher/PointMatcher.h @@ -155,9 +155,13 @@ struct PointMatcher // eigen and nabo-based types // --------------------------------- + //! The dimension of the point clouds that libpointmatcher will process + static constexpr Eigen::Index kPointDimension{ 3 }; //! The scalar type typedef T ScalarType; //! A vector over ScalarType + typedef typename Eigen::Matrix StaticCoordVector; + //! A vector over ScalarType typedef typename Eigen::Matrix Vector; //! A vector of vector over ScalarType, not a matrix typedef std::vector > VectorVector; @@ -173,6 +177,8 @@ struct PointMatcher typedef typename Eigen::Matrix Int64Matrix; //! A dense array over ScalarType typedef typename Eigen::Array Array; + //! An affine transform over ScalarType + typedef typename Eigen::Transform AffineTransform; //! A matrix holding the parameters a transformation. @@ -766,6 +772,98 @@ struct PointMatcher TransformationParameters T_refIn_refMean; //!< offset for centered map }; + // + // This class contains methods to generate point clouds in the shape of geometric primitives. + // + struct PointCloudGenerator + { + using Index = typename DataPoints::Index; + + //! @brief Builds a 3D affine transformation with a given translation and rotation. + //! + //! @param translation Translation. [m] + //! @param rotation Rotation quaternion. + //! @return AffineTransform Resulting transformation. + static AffineTransform buildUpTransformation(const StaticCoordVector& translation, const Quaternion& rotation); + + //! @brief Adds 3D coordinates and normals fields to a point cloud. + //! + //! @param numberOfPoints[in] Number of points to add to the point cloud. + //! @param pointCloud[out] Point cloud. + static void addEmpty3dPointFields(const Index numberOfPoints, DataPoints& pointCloud); + + //! @brief Transforms a point cloud by translating and rotating it a given amount. + //! + //! @param translation[in] Translation. + //! @param rotation[in] Rotation. + //! @param pointCloud[out] Point cloud to transform. + static void applyTransformation(const StaticCoordVector& translation, const Quaternion& rotation, DataPoints& pointCloud); + + //! @brief Computes a normal vector from a vector that contains the dimensions of a 2D shape in at-most 2 directions. + //! + //! @param axisAlignedPlaneDimensions[in] Dimensions vector. + //! @return StaticCoordVector Normal vector. + static StaticCoordVector computeNormalOfAxisAlignedPlane(const StaticCoordVector& axisAlignedPlaneDimensions); + + //! @brief Generates a uniformly sampled sphere (with no filling), with a given number of points and pose. + //! + //! @param radius[in] Radius of the sphere. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the sphere origin, to be used for positioning the sphere. + //! @param rotation[in] Rotation with respect to the sphere origin, to be used for positioning the sphere. + //! @return DataPoints Sphere's point cloud. + static DataPoints generateUniformlySampledSphere(const ScalarType radius, const Index numberOfPoints, + const StaticCoordVector& translation, const Quaternion& rotation); + + //! @brief Generates a uniformly sampled circle, with a given number of points and pose. + //! + //! @param radius[in] Radius of the circle. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the circle origin, to be used for positioning the circle. + //! @param rotation[in] Rotation with respect to the circle origin, to be used for positioning the circle. + //! @return DataPoints Circle's point cloud. + static DataPoints generateUniformlySampledCircle(const ScalarType radius, const Index numberOfPoints, + const StaticCoordVector& translation, const Quaternion& rotation); + + //! @brief Generates a uniformly sampled cylinder (with no filling), with a given number of points and pose. + //! + //! @param radius[in] Radius of the cylinder. [m] + //! @param height[in] Height of the cylinder. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the cylinder origin, to be used for positioning the cylinder. + //! @param rotation[in] Rotation with respect to the cylinder origin, to be used for positioning the cylinder. + //! @return DataPoints Circle's point cloud. + static DataPoints generateUniformlySampledCylinder(const ScalarType radius, const ScalarType height, const Index numberOfPoints, + const StaticCoordVector& translation, const Quaternion& rotation); + + + //! @brief Generates a uniformly sampled plane, with a given number of points and pose. + //! + //! @param dimensions[in] Dimensions of the plane (length, width, height). [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the plane origin, to be used for positioning the plane. + //! @param rotation[in] Rotation with respect to the plane origin, to be used for positioning the plane. + //! @return DataPoints Plane's point cloud. + static DataPoints generateUniformlySampledPlane(const StaticCoordVector& dimensions, const Index numberOfPoints, + const StaticCoordVector& translation, const Quaternion& rotation); + + //! @brief Generates a uniformly sampled box (with no filling), with a given number of points and pose. + //! + //! @param length[in] Length of the box. [m] + //! @param width[in] Width of the box. [m] + //! @param height[in] Height of the box. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the box origin, to be used for positioning the box. + //! @param rotation[in] Rotation with respect to the box origin, to be used for positioning the box. + //! @return DataPoints Box's point cloud. + static DataPoints generateUniformlySampledBox(const ScalarType length, const ScalarType width, const ScalarType height, + const Index numberOfPoints, const StaticCoordVector& translation, + const Quaternion& rotation); + + //! Pi. + static constexpr ScalarType pi{ static_cast(M_PI) }; + }; + // --------------------------------- // Instance-related functions // --------------------------------- diff --git a/utest/CMakeLists.txt b/utest/CMakeLists.txt index 8354fd860..a8495a544 100644 --- a/utest/CMakeLists.txt +++ b/utest/CMakeLists.txt @@ -8,6 +8,7 @@ add_executable(utest ui/Outliers.cpp ui/ErrorMinimizers.cpp ui/Transformations.cpp + ui/PointCloudGenerator.cpp ui/DataPoints.cpp ui/Inspectors.cpp ui/Loggers.cpp) diff --git a/utest/ui/PointCloudGenerator.cpp b/utest/ui/PointCloudGenerator.cpp new file mode 100644 index 000000000..677171a4d --- /dev/null +++ b/utest/ui/PointCloudGenerator.cpp @@ -0,0 +1,233 @@ + +#include "../utest.h" + +class PointCloudGeneratorTest : public ::testing::Test +{ +public: + PointCloudGeneratorTest() = default; + + /* Setup methods */ + void setDefaultParameters() + { + translation_ = PM::StaticCoordVector(0.0f, 0.5f, 0.0f); + orientation_ = PM::Quaternion(0.0f, 0.2f, 5.0f, 1.0f); + orientation_.normalize(); + + numberOfPoints_ = 10000; + } + + // Parameters. + PM::StaticCoordVector translation_{ PM::StaticCoordVector::Zero() }; + PM::Quaternion orientation_{ PM::Quaternion::Identity() }; + PM::DataPoints::Index numberOfPoints_{ 0 }; + + // Error tolerance. + const PM::ScalarType kEpsilonError_{ 1e-5 }; +}; + +// This test validates that the function that builds up transformations to point clouds is correct. Considers pure translation +TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly) +{ // NOLINT + const PM::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; + const PM::Quaternion orientation{ 0.0f, 0.0f, 0.0f, 1.0f }; + + // Build up transformation. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + + // Assertions on results. + ASSERT_EQ(transformation.translation(), translation); + ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); +} + +// This test validates that the function that builds up transformations to point clouds is correct. Considers pure rotation. +TEST_F(PointCloudGeneratorTest, BuildUpTransformationRotationOnly) +{ // NOLINT + const PM::StaticCoordVector translation{ 0.0f, 0.0f, 0.0f }; + const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f }; + + // Build up transformation. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + + // Assertions on results. + ASSERT_EQ(transformation.translation(), translation); + ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); +} + +// This test validates that the function that builds up transformations to point clouds is correct. Considers translation+rotation. +TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationRotation) +{ // NOLINT + const PM::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; + const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f }; + + // Build up transformation. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + + // Assertions on results. + ASSERT_EQ(transformation.translation(), translation); + ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); +} + +// This test validates that the function that creates empty 3D point clouds is correct. +TEST_F(PointCloudGeneratorTest, AddEmpty3dPointFields) +{ // NOLINT + PM::DataPoints pointCloud; + PM::PointCloudGenerator::addEmpty3dPointFields(numberOfPoints_, pointCloud); + + // Assertions on results. + // Number of points. + ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); + // Feature labels. + ASSERT_TRUE(pointCloud.featureExists(std::string("x"))); + ASSERT_TRUE(pointCloud.featureExists(std::string("y"))); + ASSERT_TRUE(pointCloud.featureExists(std::string("z"))); + ASSERT_TRUE(pointCloud.featureLabels.size()); +} + +// This test validates that the function that applies transformations to point clouds is correct. +TEST_F(PointCloudGeneratorTest, ApplyTransformation) +{ // NOLINT + // Test points. + const PM::DataPoints::Index numberOfPoints{ 2 }; + const PM::StaticCoordVector point1{ 0.0f, 0.0f, 0.0f }; + const PM::StaticCoordVector point2{ 2.1213f, -100000.0f, -23459999.2342312370987978687f }; + // First point is at the origin, second is somewhere else. + + // Point cloud. + PM::DataPoints pointCloud; + PM::PointCloudGenerator::addEmpty3dPointFields(numberOfPoints, pointCloud); + pointCloud.features(0, 0) = point1(0); + pointCloud.features(1, 0) = point1(1); + pointCloud.features(2, 0) = point1(2); + pointCloud.features(0, 1) = point2(0); + pointCloud.features(1, 1) = point2(1); + pointCloud.features(2, 1) = point2(2); + + // Build up transformation. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + + // Transform point cloud and points. + PM::PointCloudGenerator::applyTransformation(translation_, orientation_, pointCloud); + const PM::StaticCoordVector transformedPoint1{ transformation * point1 }; + const PM::StaticCoordVector transformedPoint2{ transformation * point2 }; + + // Assertions on results. + // Number of points. + ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints); + // Value of transformed points. + ASSERT_EQ(pointCloud.features(0, 0), transformedPoint1(0)); + ASSERT_EQ(pointCloud.features(1, 0), transformedPoint1(1)); + ASSERT_EQ(pointCloud.features(2, 0), transformedPoint1(2)); + ASSERT_EQ(pointCloud.features(0, 1), transformedPoint2(0)); + ASSERT_EQ(pointCloud.features(1, 1), transformedPoint2(1)); + ASSERT_EQ(pointCloud.features(2, 1), transformedPoint2(2)); +} + +// This test validates the construction of base shape attributes through a derived class. +TEST_F(PointCloudGeneratorTest, SphereShape) +{ // NOLINT + setDefaultParameters(); + + // Dimensions of the sphere. + const PM::ScalarType radius{ 1 }; + + // Generate point cloud. + const PM::DataPoints pointCloud{ PM::PointCloudGenerator::generateUniformlySampledSphere( + radius, numberOfPoints_, translation_, orientation_) }; + + // Assertions on results. + // Number of points. + ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); + // Points correspond to volume. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + bool isSphere{ true }; + const PM::ScalarType expectedRadiusSquared{ radius * radius }; + for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isSphere; ++i) + { + // Fetch point and remove transformation offset. + const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + + // Check whether the point lies inside the volume. + const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1) + + centeredPoint(2) * centeredPoint(2) }; + // For the point to belong to a sphere, its radius from the center must be within the expected margin. + if (computedRadiusSquared > expectedRadiusSquared + kEpsilonError_) + { + isSphere = false; + } + } + ASSERT_TRUE(isSphere); +} + +TEST_F(PointCloudGeneratorTest, CylinderShape) +{ // NOLINT + setDefaultParameters(); + + // Dimensions of the cylinder. + const PM::ScalarType radius{ 1 }; + const PM::ScalarType height{ 2 }; + + // Generate point cloud. + const PM::DataPoints pointCloud{ PM::PointCloudGenerator::generateUniformlySampledCylinder( + radius, height, numberOfPoints_, translation_, orientation_) }; + + // Assertions on results. + // Number of points. + ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); + // Points correspond to volume. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + bool isCylinder{ true }; + const PM::ScalarType expectedRadiusSquared = radius * radius; + for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCylinder; ++i) + { + // Fetch point and remove transformation offset. + const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + + // Check whether the point lies inside the volume. + const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1) }; + const PM::ScalarType computedHeight{ std::abs(centeredPoint(2)) }; + // For the point to belong to a cylinder, its 2D section (circle) must have the right radius, and its height must be within the + // expected margin. + if (computedRadiusSquared > expectedRadiusSquared + kEpsilonError_ || computedHeight > height + kEpsilonError_) + { + isCylinder = false; + } + } + ASSERT_TRUE(isCylinder); +} + +TEST_F(PointCloudGeneratorTest, BoxShape) +{ // NOLINT + setDefaultParameters(); + + // Dimensions of the box. + const PM::ScalarType length{ 1 }; + const PM::ScalarType width{ 2 }; + const PM::ScalarType height{ 5 }; + + // Generate point cloud. + const PM::DataPoints pointCloud{ PM::PointCloudGenerator::generateUniformlySampledBox( + length, width, height, numberOfPoints_, translation_, orientation_) }; + + // Assertions on results. + // Number of points. + ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); + // Points correspond to volume. + const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + bool isCube{ true }; + for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCube; ++i) + { + // Fetch point and remove transformation offset. + const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + + // Check whether the point lies inside the volume. + if (std::abs(centeredPoint(0)) > 0.5f * length + kEpsilonError_ || std::abs(centeredPoint(1)) > 0.5f * width + kEpsilonError_ + || std::abs(centeredPoint(2)) > 0.5f * height + kEpsilonError_) + { + isCube = false; + } + } + ASSERT_TRUE(isCube); +} From 376743fc670db3547b7bdee3cb8b69dcd8149017 Mon Sep 17 00:00:00 2001 From: boxanm Date: Thu, 2 May 2024 13:45:14 -0400 Subject: [PATCH 2/9] Add yaml-cpp as a dependency in libpointmatcherConfig.cmake.in --- libpointmatcherConfig.cmake.in | 1 + 1 file changed, 1 insertion(+) diff --git a/libpointmatcherConfig.cmake.in b/libpointmatcherConfig.cmake.in index 1feb40084..040616663 100644 --- a/libpointmatcherConfig.cmake.in +++ b/libpointmatcherConfig.cmake.in @@ -6,6 +6,7 @@ include(CMakeFindDependencyMacro) find_dependency(libnabo REQUIRED) +find_dependency(yaml-cpp REQUIRED) find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED) if (Boost_MINOR_VERSION GREATER 47) find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono REQUIRED) From b9169818f14531ffef3e03a1abce7f8656acff8d Mon Sep 17 00:00:00 2001 From: boxanm Date: Thu, 2 May 2024 14:31:04 -0400 Subject: [PATCH 3/9] Fix compilation error in PointCloudGenerator.cpp test caused by Eigen API changes --- utest/ui/PointCloudGenerator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/utest/ui/PointCloudGenerator.cpp b/utest/ui/PointCloudGenerator.cpp index 677171a4d..39b063d5b 100644 --- a/utest/ui/PointCloudGenerator.cpp +++ b/utest/ui/PointCloudGenerator.cpp @@ -36,7 +36,7 @@ TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly) // Assertions on results. ASSERT_EQ(transformation.translation(), translation); - ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); + ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix())); } // This test validates that the function that builds up transformations to point clouds is correct. Considers pure rotation. @@ -50,7 +50,7 @@ TEST_F(PointCloudGeneratorTest, BuildUpTransformationRotationOnly) // Assertions on results. ASSERT_EQ(transformation.translation(), translation); - ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); + ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix())); } // This test validates that the function that builds up transformations to point clouds is correct. Considers translation+rotation. @@ -64,7 +64,7 @@ TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationRotation) // Assertions on results. ASSERT_EQ(transformation.translation(), translation); - ASSERT_EQ(transformation.linear(), orientation.normalized().toRotationMatrix()); + ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix())); } // This test validates that the function that creates empty 3D point clouds is correct. From bf2c2e5a79db25cb18c2b2adab159faba89643da Mon Sep 17 00:00:00 2001 From: boxanm Date: Fri, 3 May 2024 14:40:00 -0400 Subject: [PATCH 4/9] Add python bindings for Point Cloud Generator class --- python/CMakeLists.txt | 1 + python/modules/point_matcher_module.cpp | 2 + python/pointmatcher/point_cloud_generator.cpp | 126 ++++++++++++++++++ python/pointmatcher/point_cloud_generator.h | 19 +++ python/pypoint_matcher_helper.h | 5 + 5 files changed, 153 insertions(+) create mode 100644 python/pointmatcher/point_cloud_generator.cpp create mode 100644 python/pointmatcher/point_cloud_generator.h diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d4ab984b2..64e88fd36 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -20,6 +20,7 @@ set(PYBIND11_SOURCES pointmatcher/outlier_filter.cpp pointmatcher/outlier_filters.cpp pointmatcher/point_matcher.cpp + pointmatcher/point_cloud_generator.cpp pointmatcher/transformation.cpp pointmatcher/transformations.cpp pointmatcher/transformation_checker.cpp diff --git a/python/modules/point_matcher_module.cpp b/python/modules/point_matcher_module.cpp index 349f871c0..526edd2b0 100644 --- a/python/modules/point_matcher_module.cpp +++ b/python/modules/point_matcher_module.cpp @@ -3,6 +3,7 @@ #include "pointmatcher/point_matcher.h" #include "pointmatcher/impl.h" #include "pointmatcher/io.h" +#include "pointmatcher/point_cloud_generator.h" namespace python { @@ -15,6 +16,7 @@ namespace python pointmatcher::pybindPointMatcher(pointmatcherModule); pointmatcher::pybindIO(pointmatcherModule); pointmatcher::pybindImpl(pointmatcherModule); + pointmatcher::pybindPointCloudGenerator(pointmatcherModule); } } } diff --git a/python/pointmatcher/point_cloud_generator.cpp b/python/pointmatcher/point_cloud_generator.cpp new file mode 100644 index 000000000..cf6e53519 --- /dev/null +++ b/python/pointmatcher/point_cloud_generator.cpp @@ -0,0 +1,126 @@ +// +// Created by Matěj Boxan on 2024-05-02. +// + +#include "point_cloud_generator.h" + +#include "pointmatcher/PointMatcher.h" + +namespace python +{ + namespace pointmatcher + { + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param translation[in] Translation. + //! @param rotation[in] Rotation. + //! @param pointCloud[out] Point cloud to transform + void applyTransformation( + const StaticCoordVector& translation, const Eigen::Matrix& rotation, + DataPoints& pointCloud) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + Generator::applyTransformation(translation, quaternion, pointCloud); + } + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param length[in] Length of the box. [m] + //! @param width[in] Width of the box. [m] + //! @param height[in] Height of the box. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the box origin, to be used for positioning the box. + //! @param rotation[in] Rotation with respect to the box origin, to be used for positioning the box. + //! @return DataPoints Box's point cloud. + DataPoints generateUniformlySampledBoxWrapper( + const ScalarType length, const ScalarType width, const ScalarType height, const DataPoints::Index numberOfPoints, + const StaticCoordVector& translation, const Eigen::Matrix& rotation) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + return Generator::generateUniformlySampledBox(length, width, height, numberOfPoints, translation, quaternion); + } + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param dimensions[in] Dimensions of the plane (length, width, height). [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the plane origin, to be used for positioning the plane. + //! @param rotation[in] Rotation with respect to the plane origin, to be used for positioning the plane. + //! @return DataPoints Plane's point cloud. + DataPoints generateUniformlySampledPlane( + const StaticCoordVector& dimensions, const DataPoints::Index numberOfPoints, + const StaticCoordVector& translation, const Eigen::Matrix& rotation) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + return Generator::generateUniformlySampledPlane(dimensions, numberOfPoints, translation, quaternion); + } + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param radius[in] Radius of the cylinder. [m] + //! @param height[in] Height of the cylinder. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the cylinder origin, to be used for positioning the cylinder. + //! @param rotation[in] Rotation with respect to the cylinder origin, to be used for positioning the cylinder. + //! @return DataPoints Circle's point cloud. + DataPoints generateUniformlySampledCylinder( + const ScalarType radius, const ScalarType height, const DataPoints::Index numberOfPoints, + const StaticCoordVector& translation, const Eigen::Matrix& rotation) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + return Generator::generateUniformlySampledCylinder(radius, height, numberOfPoints, translation, quaternion); + } + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param radius[in] Radius of the circle. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the circle origin, to be used for positioning the circle. + //! @param rotation[in] Rotation with respect to the circle origin, to be used for positioning the circle. + //! @return DataPoints Circle's point cloud. + DataPoints generateUniformlySampledCircle( + const ScalarType radius, const DataPoints::Index numberOfPoints, + const StaticCoordVector& translation, const Eigen::Matrix& rotation) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + return Generator::generateUniformlySampledCircle(radius, numberOfPoints, translation, quaternion); + } + + //! @brief Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion. + //! + //! @param radius[in] Radius of the sphere. [m] + //! @param numberOfPoints[in] Number of points. + //! @param translation[in] Translation with respect to the sphere origin, to be used for positioning the sphere. + //! @return DataPoints Sphere's point cloud. + DataPoints generateUniformlySampledSphere( + const ScalarType radius, const DataPoints::Index numberOfPoints, + const StaticCoordVector& translation, const Eigen::Matrix& rotation) + { + PM::Quaternion quaternion(rotation(0), rotation(1), rotation(2), rotation(3)); + return Generator::generateUniformlySampledSphere(radius, numberOfPoints, translation, quaternion); + } + + void pybindPointCloudGenerator(py::module& p_module) + { + py::class_(p_module, "PointCloudGenerator", "Class containing methods to generate point clouds in the shape of geometric primitives.") + .def_static("buildUpTransformation", + &Generator::buildUpTransformation, + py::arg("translation"), + py::arg("rotation")) + .def_static("addEmpty3dPointFields", &Generator::addEmpty3dPointFields, py::arg("numberOfPoints"), py::arg("pointCloud")) + .def_static("applyTransformation", &applyTransformation, py::arg("translation"), py::arg("rotation"), py::arg("pointCloud")) + .def_static("computeNormalOfAxisAlignedPlane", &Generator::computeNormalOfAxisAlignedPlane, py::arg("axisAlignedPlaneDimensions")) + .def_static("generateUniformlySampledSphere", &generateUniformlySampledSphere, py::arg("radius"), py::arg("numberOfPoints"), py::arg("translation"), + py::arg("rotation")) + .def_static("generateUniformlySampledCircle", &generateUniformlySampledCircle, py::arg("radius"), py::arg("numberOfPoints"), py::arg("translation"), + py::arg("rotation")) + .def_static("generateUniformlySampledCylinder", &generateUniformlySampledCylinder, py::arg("radius"), py::arg("height"), py::arg("numberOfPoints"), + py::arg("translation"), py::arg("rotation")) + .def_static("generateUniformlySampledPlane", &generateUniformlySampledPlane, py::arg("dimensions"), py::arg("numberOfPoints"), + py::arg("translation"), py::arg("rotation")) + .def_static("generateUniformlySampledBox", &generateUniformlySampledBoxWrapper, py::arg("length"), py::arg("width"), py::arg("height"), + py::arg("numberOfPoints"), py::arg("translation"), py::arg("rotation")); + } + } +} \ No newline at end of file diff --git a/python/pointmatcher/point_cloud_generator.h b/python/pointmatcher/point_cloud_generator.h new file mode 100644 index 000000000..a30e5543d --- /dev/null +++ b/python/pointmatcher/point_cloud_generator.h @@ -0,0 +1,19 @@ +// +// Created by Matěj Boxan on 2024-05-02. +// + +#ifndef LIBPOINTMATCHER_POINT_CLOUD_GENERATOR_H +#define LIBPOINTMATCHER_POINT_CLOUD_GENERATOR_H + + +#include "pypoint_matcher_helper.h" + +namespace python +{ + namespace pointmatcher + { + void pybindPointCloudGenerator(py::module& p_module); + } +} + +#endif //LIBPOINTMATCHER_POINT_CLOUD_GENERATOR_H diff --git a/python/pypoint_matcher_helper.h b/python/pypoint_matcher_helper.h index 12415fada..567c4eff1 100644 --- a/python/pypoint_matcher_helper.h +++ b/python/pypoint_matcher_helper.h @@ -63,6 +63,11 @@ using Array = PM::Array; using TransformationParameters = PM::TransformationParameters; using OutlierWeights = PM::OutlierWeights; +// Point Cloud Generator +using Generator = PM::PointCloudGenerator; +using AffineTransform = PM::AffineTransform; +using StaticCoordVector = PM::StaticCoordVector; + PYBIND11_MAKE_OPAQUE(std::vector) // StringVector PYBIND11_MAKE_OPAQUE(std::map>) // Bibliography PYBIND11_MAKE_OPAQUE(std::map) // BibIndices From 1ec55f84125308aaeba0b47685ff863d88867901 Mon Sep 17 00:00:00 2001 From: boxanm Date: Fri, 3 May 2024 14:40:39 -0400 Subject: [PATCH 5/9] Add example python script for Point Cloud Generator class --- examples/python/point_cloud_generator.py | 50 ++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 examples/python/point_cloud_generator.py diff --git a/examples/python/point_cloud_generator.py b/examples/python/point_cloud_generator.py new file mode 100644 index 000000000..ce67b189f --- /dev/null +++ b/examples/python/point_cloud_generator.py @@ -0,0 +1,50 @@ +# Code example for ICP generating different shapes using the built-in interface +import os.path + +import numpy as np +from pypointmatcher import pointmatcher as pm + +PM = pm.PointMatcher +DP = PM.DataPoints +Generator = pm.PointCloudGenerator + +# Path of output directory (default: tests/icp_simple/) +# The output directory must already exist +# Leave empty to save in the current directory +output_base_directory = "tests/generator/" + +# How many points will each generated shape contain +number_of_points = 10000 + +# Toggle to switch between 2D and 3D clouds +# Only 3D is currently supported +is_3D = True + +if is_3D: + # Load 3D point clouds + ref = DP(DP.load('../data/car_cloud400.csv')) + data = DP(DP.load('../data/car_cloud401.csv')) + test_base = "3D" + translation = np.array([[0], [0], [0]]) + rotation = np.array([1, 0, 0, 0], dtype=np.float32) +else: + raise Exception("The Point Cloud Generator only supports 3D shapes") + +box = Generator.generateUniformlySampledBox(1.0, 2.0, 3.0, number_of_points, translation, rotation) +circle = Generator.generateUniformlySampledCircle(1.0, number_of_points, translation, rotation) +cylinder = Generator.generateUniformlySampledCylinder(1.0, 2.0, number_of_points, translation, rotation) +plane = Generator.generateUniformlySampledPlane(np.array([1.0, 2.0, 3.0]), number_of_points, translation, rotation) +sphere = Generator.generateUniformlySampledSphere(1.0, number_of_points, translation, rotation) + + +# Save files to see the results +if not os.path.exists(output_base_directory): + os.makedirs(output_base_directory) + +box.save(f"{output_base_directory}box.vtk") +circle.save(f"{output_base_directory}circle.vtk") +cylinder.save(f"{output_base_directory}cylinder.vtk") +plane.save(f"{output_base_directory}plane.vtk") +sphere.save(f"{output_base_directory}sphere.vtk") + +print(f"Saved generated shapes into {output_base_directory}") From d9f4846b6cfc94893fcb1dc1b1bd33016a9415ae Mon Sep 17 00:00:00 2001 From: boxanm Date: Fri, 3 May 2024 14:41:01 -0400 Subject: [PATCH 6/9] Add .cpp extension to PointCloudGenerator in CMakeLists.txt --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1df9c8c7f..0acebe370 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -343,7 +343,7 @@ set(POINTMATCHER_SRC pointmatcher/DataPointsFilters/Saliency.cpp pointmatcher/DataPointsFilters/SpectralDecomposition.cpp #PointCloudGenerators - pointmatcher/PointCloudGenerator + pointmatcher/PointCloudGenerator.cpp ) From f4bcbf2bd45292388e94c26e61ee5c1d78ded6f8 Mon Sep 17 00:00:00 2001 From: boxanm Date: Fri, 3 May 2024 15:02:18 -0400 Subject: [PATCH 7/9] Move 3D-specific types to PointCloudGenerator class --- pointmatcher/PointCloudGenerator.cpp | 4 +-- pointmatcher/PointMatcher.h | 15 +++++----- python/pypoint_matcher_helper.h | 4 +-- utest/ui/PointCloudGenerator.cpp | 44 ++++++++++++++-------------- 4 files changed, 34 insertions(+), 33 deletions(-) diff --git a/pointmatcher/PointCloudGenerator.cpp b/pointmatcher/PointCloudGenerator.cpp index b8f6dce85..010e5f463 100644 --- a/pointmatcher/PointCloudGenerator.cpp +++ b/pointmatcher/PointCloudGenerator.cpp @@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include template -typename PointMatcher::AffineTransform PointMatcher::PointCloudGenerator::buildUpTransformation(const StaticCoordVector& translation, +typename PointMatcher::PointCloudGenerator::AffineTransform PointMatcher::PointCloudGenerator::buildUpTransformation(const StaticCoordVector& translation, const Quaternion& rotation) { AffineTransform transformation; @@ -81,7 +81,7 @@ void PointMatcher::PointCloudGenerator::applyTransformation(const StaticCoord } template -typename PointMatcher::StaticCoordVector PointMatcher::PointCloudGenerator::computeNormalOfAxisAlignedPlane( +typename PointMatcher::PointCloudGenerator::StaticCoordVector PointMatcher::PointCloudGenerator::computeNormalOfAxisAlignedPlane( const StaticCoordVector& axisAlignedPlaneDimensions) { StaticCoordVector normalVector{ StaticCoordVector::Zero() }; diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h index 5f5668b00..1cfcd3c96 100644 --- a/pointmatcher/PointMatcher.h +++ b/pointmatcher/PointMatcher.h @@ -154,14 +154,10 @@ struct PointMatcher // --------------------------------- // eigen and nabo-based types // --------------------------------- - - //! The dimension of the point clouds that libpointmatcher will process - static constexpr Eigen::Index kPointDimension{ 3 }; + //! The scalar type typedef T ScalarType; //! A vector over ScalarType - typedef typename Eigen::Matrix StaticCoordVector; - //! A vector over ScalarType typedef typename Eigen::Matrix Vector; //! A vector of vector over ScalarType, not a matrix typedef std::vector > VectorVector; @@ -177,8 +173,6 @@ struct PointMatcher typedef typename Eigen::Matrix Int64Matrix; //! A dense array over ScalarType typedef typename Eigen::Array Array; - //! An affine transform over ScalarType - typedef typename Eigen::Transform AffineTransform; //! A matrix holding the parameters a transformation. @@ -777,6 +771,13 @@ struct PointMatcher // struct PointCloudGenerator { + //! The dimension of the point clouds that libpointmatcher will process + static constexpr Eigen::Index kPointDimension{ 3 }; + //! An affine transform over ScalarType of kPointDimension + typedef typename Eigen::Transform AffineTransform; + //! A vector over ScalarType of kPointDimension + typedef typename Eigen::Matrix StaticCoordVector; + using Index = typename DataPoints::Index; //! @brief Builds a 3D affine transformation with a given translation and rotation. diff --git a/python/pypoint_matcher_helper.h b/python/pypoint_matcher_helper.h index 567c4eff1..31ed56faa 100644 --- a/python/pypoint_matcher_helper.h +++ b/python/pypoint_matcher_helper.h @@ -65,8 +65,8 @@ using OutlierWeights = PM::OutlierWeights; // Point Cloud Generator using Generator = PM::PointCloudGenerator; -using AffineTransform = PM::AffineTransform; -using StaticCoordVector = PM::StaticCoordVector; +using AffineTransform = Generator::AffineTransform; +using StaticCoordVector = Generator::StaticCoordVector; PYBIND11_MAKE_OPAQUE(std::vector) // StringVector PYBIND11_MAKE_OPAQUE(std::map>) // Bibliography diff --git a/utest/ui/PointCloudGenerator.cpp b/utest/ui/PointCloudGenerator.cpp index 39b063d5b..da98a46a5 100644 --- a/utest/ui/PointCloudGenerator.cpp +++ b/utest/ui/PointCloudGenerator.cpp @@ -9,7 +9,7 @@ class PointCloudGeneratorTest : public ::testing::Test /* Setup methods */ void setDefaultParameters() { - translation_ = PM::StaticCoordVector(0.0f, 0.5f, 0.0f); + translation_ = PM::PointCloudGenerator::StaticCoordVector(0.0f, 0.5f, 0.0f); orientation_ = PM::Quaternion(0.0f, 0.2f, 5.0f, 1.0f); orientation_.normalize(); @@ -17,7 +17,7 @@ class PointCloudGeneratorTest : public ::testing::Test } // Parameters. - PM::StaticCoordVector translation_{ PM::StaticCoordVector::Zero() }; + PM::PointCloudGenerator::StaticCoordVector translation_{ PM::PointCloudGenerator::StaticCoordVector::Zero() }; PM::Quaternion orientation_{ PM::Quaternion::Identity() }; PM::DataPoints::Index numberOfPoints_{ 0 }; @@ -28,11 +28,11 @@ class PointCloudGeneratorTest : public ::testing::Test // This test validates that the function that builds up transformations to point clouds is correct. Considers pure translation TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly) { // NOLINT - const PM::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; + const PM::PointCloudGenerator::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; const PM::Quaternion orientation{ 0.0f, 0.0f, 0.0f, 1.0f }; // Build up transformation. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; // Assertions on results. ASSERT_EQ(transformation.translation(), translation); @@ -42,11 +42,11 @@ TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly) // This test validates that the function that builds up transformations to point clouds is correct. Considers pure rotation. TEST_F(PointCloudGeneratorTest, BuildUpTransformationRotationOnly) { // NOLINT - const PM::StaticCoordVector translation{ 0.0f, 0.0f, 0.0f }; + const PM::PointCloudGenerator::StaticCoordVector translation{ 0.0f, 0.0f, 0.0f }; const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f }; // Build up transformation. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; // Assertions on results. ASSERT_EQ(transformation.translation(), translation); @@ -56,11 +56,11 @@ TEST_F(PointCloudGeneratorTest, BuildUpTransformationRotationOnly) // This test validates that the function that builds up transformations to point clouds is correct. Considers translation+rotation. TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationRotation) { // NOLINT - const PM::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; + const PM::PointCloudGenerator::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f }; const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f }; // Build up transformation. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) }; // Assertions on results. ASSERT_EQ(transformation.translation(), translation); @@ -88,8 +88,8 @@ TEST_F(PointCloudGeneratorTest, ApplyTransformation) { // NOLINT // Test points. const PM::DataPoints::Index numberOfPoints{ 2 }; - const PM::StaticCoordVector point1{ 0.0f, 0.0f, 0.0f }; - const PM::StaticCoordVector point2{ 2.1213f, -100000.0f, -23459999.2342312370987978687f }; + const PM::PointCloudGenerator::StaticCoordVector point1{ 0.0f, 0.0f, 0.0f }; + const PM::PointCloudGenerator::StaticCoordVector point2{ 2.1213f, -100000.0f, -23459999.2342312370987978687f }; // First point is at the origin, second is somewhere else. // Point cloud. @@ -103,12 +103,12 @@ TEST_F(PointCloudGeneratorTest, ApplyTransformation) pointCloud.features(2, 1) = point2(2); // Build up transformation. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; // Transform point cloud and points. PM::PointCloudGenerator::applyTransformation(translation_, orientation_, pointCloud); - const PM::StaticCoordVector transformedPoint1{ transformation * point1 }; - const PM::StaticCoordVector transformedPoint2{ transformation * point2 }; + const PM::PointCloudGenerator::StaticCoordVector transformedPoint1{ transformation * point1 }; + const PM::PointCloudGenerator::StaticCoordVector transformedPoint2{ transformation * point2 }; // Assertions on results. // Number of points. @@ -138,14 +138,14 @@ TEST_F(PointCloudGeneratorTest, SphereShape) // Number of points. ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); // Points correspond to volume. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; bool isSphere{ true }; const PM::ScalarType expectedRadiusSquared{ radius * radius }; for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isSphere; ++i) { // Fetch point and remove transformation offset. - const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); - const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point }; // Check whether the point lies inside the volume. const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1) @@ -175,14 +175,14 @@ TEST_F(PointCloudGeneratorTest, CylinderShape) // Number of points. ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); // Points correspond to volume. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; bool isCylinder{ true }; const PM::ScalarType expectedRadiusSquared = radius * radius; for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCylinder; ++i) { // Fetch point and remove transformation offset. - const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); - const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point }; // Check whether the point lies inside the volume. const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1) }; @@ -214,13 +214,13 @@ TEST_F(PointCloudGeneratorTest, BoxShape) // Number of points. ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_); // Points correspond to volume. - const PM::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; + const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) }; bool isCube{ true }; for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCube; ++i) { // Fetch point and remove transformation offset. - const PM::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); - const PM::StaticCoordVector centeredPoint{ transformation.inverse() * point }; + const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2)); + const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point }; // Check whether the point lies inside the volume. if (std::abs(centeredPoint(0)) > 0.5f * length + kEpsilonError_ || std::abs(centeredPoint(1)) > 0.5f * width + kEpsilonError_ From 82e61490dee95747ff4cb8f3a2cb33112c942358 Mon Sep 17 00:00:00 2001 From: boxanm Date: Mon, 6 May 2024 11:02:51 -0400 Subject: [PATCH 8/9] Add a square root for radius to have uniform density of points in generated circles --- pointmatcher/PointCloudGenerator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointmatcher/PointCloudGenerator.cpp b/pointmatcher/PointCloudGenerator.cpp index 010e5f463..5bb0d9ad7 100644 --- a/pointmatcher/PointCloudGenerator.cpp +++ b/pointmatcher/PointCloudGenerator.cpp @@ -170,7 +170,7 @@ typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::gener { // Sample random values of theta and phi. const ScalarType phi{ 2.0f * pi * uniformDistribution(randomNumberGenerator) }; - const ScalarType radiusSample{ radius * uniformDistribution(randomNumberGenerator) }; + const ScalarType radiusSample{ sqrt(radius * uniformDistribution(randomNumberGenerator)) }; // Pre-compute values, such as sine and cosine of phi and theta. const ScalarType sinPhi{ std::sin(phi) }; From 9f9b0aa9401f97d7b1e0b0cd1930dbe387d464a4 Mon Sep 17 00:00:00 2001 From: boxanm Date: Tue, 7 May 2024 16:51:26 -0400 Subject: [PATCH 9/9] Update uniformly sampled circle --- pointmatcher/PointCloudGenerator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pointmatcher/PointCloudGenerator.cpp b/pointmatcher/PointCloudGenerator.cpp index 5bb0d9ad7..d084dd2b2 100644 --- a/pointmatcher/PointCloudGenerator.cpp +++ b/pointmatcher/PointCloudGenerator.cpp @@ -170,7 +170,7 @@ typename PointMatcher::DataPoints PointMatcher::PointCloudGenerator::gener { // Sample random values of theta and phi. const ScalarType phi{ 2.0f * pi * uniformDistribution(randomNumberGenerator) }; - const ScalarType radiusSample{ sqrt(radius * uniformDistribution(randomNumberGenerator)) }; + const ScalarType radiusSample{ radius * sqrt(uniformDistribution(randomNumberGenerator)) }; // Pre-compute values, such as sine and cosine of phi and theta. const ScalarType sinPhi{ std::sin(phi) };