diff --git a/CMakeLists.txt b/CMakeLists.txt index 99d2522..93e7d7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.12) project( ForgeScan - VERSION 0.2 + VERSION 0.3 DESCRIPTION "Policies for autonomous, iterative, and fast voxelized geometric reconstruction with simulated depth cameras." LANGUAGES C CXX @@ -97,6 +97,7 @@ target_link_libraries( ${OpenCV_LIBS} Eigen3::Eigen HighFive + Open3D::Open3D ) set_target_properties( ${INTERFACE_LIBRARY} diff --git a/include/ForgeScan/Simulation/Box.hpp b/include/ForgeScan/Simulation/Box.hpp deleted file mode 100644 index 0d03a88..0000000 --- a/include/ForgeScan/Simulation/Box.hpp +++ /dev/null @@ -1,319 +0,0 @@ -#ifndef FORGE_SCAN_SIMULATION_BOX_HPP -#define FORGE_SCAN_SIMULATION_BOX_HPP - -#include - -#include "ForgeScan/Simulation/Primitive.hpp" -#include "ForgeScan/Utilities/ArgParser.hpp" -#include "ForgeScan/Utilities/Math.hpp" - -// Helper constant for saving data in HDF5 files. -// Used in this class and by Scene. -// Undefined at the end of Scene.hpp. -#define FS_HDF5_BOX_L_ATTR "length" -#define FS_HDF5_BOX_W_ATTR "width" -#define FS_HDF5_BOX_H_ATTR "hight" - - - -namespace forge_scan { -namespace simulation { - - -/// @brief A simple analytical box object. -/// @note The reference frame for a Box is located at its center and oriented so the length is -/// aligned with the X-axis, width is aligned with the Y-axis, and width is aligned with -/// the Z-axis. -struct Box : public Primitive -{ -public: - /// @brief Constructs an analytical box of the provided dimensions. - /// @param l The box's total dimension in the X-direction. - /// @param w The box's total dimension in the Y-direction. - /// @param h The box's total dimension in the Z-direction. - /// @param extr Transformation to the world frame to the center of the Box. - Box(const float& l, const float& w, const float& h, const Extrinsic& extr = Extrinsic::Identity()) - : Primitive(extr, getAABBbound(l, w, h, true), getAABBbound(l, w, h, false)), - length(l), - width(w), - height(h) - { - - } - - - /// @brief Constructor for a shared pointer to a Box. - /// @param l The box's total dimension in the X-direction. - /// @param w The box's total dimension in the Y-direction. - /// @param h The box's total dimension in the Z-direction. - /// @param extr Transformation to the world frame to the center of the Box. - /// @return Shared pointer to a Box. - static std::shared_ptr create(const float& l, const float& w, const float& h, - const Extrinsic& extr = Extrinsic::Identity()) - { - return std::shared_ptr(new Box(l, w, h, extr)); - } - - - /// @brief Constructor for a shared pointer to a Box. - /// @param parser ArgParser with arguments to construct a Box from. - /// @return Shared pointer to a Box. - static std::shared_ptr create(const utilities::ArgParser& parser) - { - Extrinsic extr = Extrinsic::Identity(); - Entity::setRotation(parser, extr); - Entity::setTranslation(parser, extr); - - return std::shared_ptr(new Box(parser.get(Box::parse_l, Box::default_length), - parser.get(Box::parse_w, Box::default_width), - parser.get(Box::parse_h, Box::default_height), extr)); - } - - - /// @return Help message for constructing a Box with ArgParser. - static std::string helpMessage() - { - return "A Box may be added with the following arguments:" - "\n\t" + Box::help_string + - "\nIf the optional arguments are not provided, the default values are:" - "\n\t" + Box::default_arguments; - } - - - // ***************************************************************************************** // - // * PUBLIC VIRTUAL METHOD OVERRIDES * // - // ***************************************************************************************** // - - - const std::string& getTypeName() const override final - { - return Box::type_name; - } - - - bool hit(const Point& start, const Point& end, float& t) const override final - { - return this->hitAABB(start, end, t); - } - - - bool isInside(const Point& input, const Extrinsic& extr) const override final - { - Point input_this_f = this->getToThisFromOther(extr) * input.homogeneous(); - return this->insideBounds(input_this_f); - } - - - bool isInside(const Point& input, const Extrinsic& extr, Point& input_this_f) const override final - { - input_this_f = this->getToThisFromOther(extr) * input.homogeneous(); - return this->insideBounds(input_this_f); - } - - - float getSignedDistance(const Point& input, const Extrinsic& extr) const override final - { - return this->getSignedDistance(this->getToThisFromOther(extr) * input.homogeneous()); - } - - - float getSignedDistance(const Point& input) const override final - { - if (this->insideBounds(input)) - { - return this->getSignedDistanceInside(input); - } - return this->getSignedDistanceOutside(input); - } - - - Point getNearestSurfacePoint(const Point& input, const Extrinsic& extr) const override final - { - return this->getNearestSurfacePoint(this->getToThisFromOther(extr) * input.homogeneous()); - } - - - Point getNearestSurfacePoint(const Point& input) const override final - { - if (this->insideBounds(input)) - { - return this->getNearestSurfacePointInside(input); - } - return this->getNearestSurfacePointOutside(input); - } - - - - // ***************************************************************************************** // - // * PUBLIC CLASS MEMBERS * // - // ***************************************************************************************** // - - - /// @brief Dimensions of the box: length in X-direction, width in Y-direction, and height it Z-direction. - const float length, width, height; - - static const float default_length, default_width, default_height; - - static const std::string parse_l, parse_w, parse_h; - - static const std::string help_string, default_arguments; - - static const std::string type_name; - -private: - // ***************************************************************************************** // - // * PRIVATE CLASS METHODS * // - // ***************************************************************************************** // - - - /// @brief Constructor helper for generating a box's AABB bounds. - /// @param l The box's total dimension in the X-direction. - /// @param w The box's total dimension in the Y-direction. - /// @param h The box's total dimension in the Z-direction. - /// @param upper If true gets upper bound. If false gets lower bound - /// @return Axis-aligned bounding box point for the upper or lower, depending on the sign of the dimension. - static Point getAABBbound(const float& l, const float& w, const float& h, const bool& upper) - { - Point bound(l, w, h); - bound.cwiseAbs(); - if (upper) - { - bound.array() *= 0.5; - } - else - { - bound.array() *= -0.5; - } - return bound; - } - - - /// @brief Saves the shapes contents into an HDF5 file format. - /// @param g_primitive Group to add data to. - void save(HighFive::Group& g_primitive) const override final - { - g_primitive.createAttribute(FS_HDF5_PRIMITIVE_TYPE_NAME_ATTR, this->getTypeName()); - g_primitive.createAttribute(FS_HDF5_BOX_L_ATTR, this->length); - g_primitive.createAttribute(FS_HDF5_BOX_W_ATTR, this->width); - g_primitive.createAttribute(FS_HDF5_BOX_H_ATTR, this->height); - } - - - /// @brief Prints information about the Box to the output stream. - /// @param out Output stream to write to. - void print(std::ostream& out) const override final - { - out << this->getTypeName() << " "; - Primitive::print(out); - out << " with dimensions (" << this->length << ", " << this->width << ", " << this->height << ")"; - } - - - // ***************************************************************************************** // - // * PRIVATE CLASS HELPER METHODS * // - // ***************************************************************************************** // - - /// @brief Calculates the signed distance for the case where a point is outside of the Box. - /// @param input_this_f Point, relative to the Box's reference frame. - /// @return Shortest distance from the box to that point. Always non-negative. - Point getNearestSurfacePointOutside(const Point& input_this_f) const - { - // For each direction if we are between the bounds then we take zero for the delta in that direction. - // Otherwise we take the maximum value. If we are less than both bounds then sign(`lower` - `input`) is positive - // while sign(`input` -`upper`) is negative, leading the call to max to store the proper sign and magnitude. - // The opposite logic lets us store the correct sign and magnitude when we are above both bounds. - float delta_x = std::max(std::max(lowerAABBbound.x() - input_this_f.x(), 0.0f), input_this_f.x() - upperAABBbound.x()); - float delta_y = std::max(std::max(lowerAABBbound.y() - input_this_f.y(), 0.0f), input_this_f.y() - upperAABBbound.y()); - float delta_z = std::max(std::max(lowerAABBbound.z() - input_this_f.z(), 0.0f), input_this_f.z() - upperAABBbound.z()); - return input_this_f + Point(delta_x, delta_y, delta_z); - } - - - /// @brief Calculates the signed distance for the case where a point is outside of the Box. - /// @param input_this_f Point, relative to the Box's reference frame. - /// @return Shortest distance from the box to that point. Always non-negative. - float getSignedDistanceOutside(const Point& input_this_f) const - { - // Uses the same logic as getNearestSurfacePointOutside but skips creating a Point object. - float delta_x = std::max(std::max(this->lowerAABBbound.x() - input_this_f.x(), 0.0f), input_this_f.x() - this->upperAABBbound.x()); - float delta_y = std::max(std::max(this->lowerAABBbound.y() - input_this_f.y(), 0.0f), input_this_f.y() - this->upperAABBbound.y()); - float delta_z = std::max(std::max(this->lowerAABBbound.z() - input_this_f.z(), 0.0f), input_this_f.z() - this->upperAABBbound.z()); - return std::sqrt(delta_x*delta_x + delta_y*delta_y + delta_z*delta_z); - } - - - /// @brief Calculates the signed distance for the case where a point is inside of the Box. - /// @param input_this_f Point, relative to the Box's reference frame. - /// @return Shortest distance from the box to that point. Always non-positive. - Point getNearestSurfacePointInside(const Point& input_this_f) const - { - using namespace utilities::math; - - // For each direction, calculate the signed distance from the internal point to both bounds and store the distance of lesser magnitude. - float face_x = smallest_magnitude(this->lowerAABBbound.x() - input_this_f.x(), this->upperAABBbound.x() - input_this_f.x()); - float face_y = smallest_magnitude(this->lowerAABBbound.y() - input_this_f.y(), this->upperAABBbound.y() - input_this_f.y()); - float face_z = smallest_magnitude(this->lowerAABBbound.z() - input_this_f.z(), this->upperAABBbound.z() - input_this_f.z()); - - // Now, compare the directions to find which has the least magnitude overall. - if (is_lesser_in_magnitude(face_x, face_y), is_lesser_in_magnitude(face_x, face_y)) - { - return input_this_f + Point(face_x, 0, 0); - - } - else if (is_lesser_in_magnitude(face_y, face_z)) - { - return input_this_f + Point(0, face_y, 0); - } - else - { - return input_this_f + Point(0, 0, face_z); - } - } - - - /// @brief Calculates the signed distance for the case where a point is inside of the Box. - /// @param input_this_f Point, relative to the Box's reference frame. - /// @return Shortest distance from the box to that point. Always non-positive. - float getSignedDistanceInside(const Point& input_this_f) const - { - // Uses similar logic to getNearestSurfacePointInside but skips creating a Point or finding the direction - // of the closest face - only finds the magnitude. - float face_x = std::max(this->lowerAABBbound.x() - input_this_f.x(), input_this_f.x() - this->upperAABBbound.x()); - float face_y = std::max(this->lowerAABBbound.y() - input_this_f.y(), input_this_f.y() - this->upperAABBbound.y()); - float face_z = std::max(this->lowerAABBbound.z() - input_this_f.z(), input_this_f.z() - this->upperAABBbound.z()); - return std::max(std::max(face_x, face_y), face_z); - } -}; - - -/// @brief String for the class name. -const std::string Box::type_name = "Box"; - -/// @brief Default dimensions of the box. -const float Box::default_length = 1.0, Box::default_width = 1.0, Box::default_height = 1.0; - -/// @brief ArgParser key for the Box length, width, and height. -const std::string Box::parse_l = std::string("--l"), - Box::parse_w = std::string("--w"), - Box::parse_h = std::string("--h"); - -/// @brief String explaining what arguments this class accepts. -const std::string Box::help_string = - Box::translation_help_string + " " + Box::rotation_help_string + - " [" + Box::parse_l + " ]" + - " [" + Box::parse_w + " ]" + - " [" + Box::parse_h + " ]"; - -/// @brief String explaining what this class's default parsed values are. -const std::string Box::default_arguments = - Box::translation_default_arguments + " " + Box::rotation_default_arguments + - " " + Box::parse_l + " " + std::to_string(Box::default_length) + - " " + Box::parse_w + " " + std::to_string(Box::default_width) + - " " + Box::parse_h + " " + std::to_string(Box::default_height); - - -} // namespace primitives -} // namespace forge_scan - - -#endif // FORGE_SCAN_SIMULATION_BOX_HPP diff --git a/include/ForgeScan/Simulation/Constructor.hpp b/include/ForgeScan/Simulation/Constructor.hpp index 9d23ffb..4a6e229 100644 --- a/include/ForgeScan/Simulation/Constructor.hpp +++ b/include/ForgeScan/Simulation/Constructor.hpp @@ -4,62 +4,130 @@ #include #include -#include "ForgeScan/Simulation/Box.hpp" -#include "ForgeScan/Simulation/Sphere.hpp" +#include +#include +#include -#include "ForgeScan/Utilities/Strings.hpp" +#include "ForgeScan/Common/Entity.hpp" namespace forge_scan { namespace simulation { -/// @brief Constructor for shared Primitive pointers based on ArgParser inputs. +/// @brief Stores extra information about a mesh file. +struct MeshInfo +{ + /// @brief File path to the mesh. + std::filesystem::path fpath; + + /// @brief Transformation applied to the mesh. + Extrinsic extr; +}; + + +/// @brief Constructor for loading meshes into a `simulation::Scene` based on ArgParser inputs. struct Constructor { - /// @brief Factory function to create different Primitive types. - /// @param parser Arguments to pass into the Primitive's create functions. - /// @return A pointer to the requested implementation of the Primitive class. - /// @throws ConstructorError if the Primitive type is not recognized. - static std::shared_ptr create(const utilities::ArgParser& parser) + /// @brief Function to read meshes into a `simulation::Scene`. + /// @param parser Arguments to select the mesh file and describe its transformation. + /// @return A pair with the MeshInfo and the TriangleMesh object. + /// @throws ConstructorError if there was an issue loading the mesh + static std::pair create(const utilities::ArgParser& parser) { - using namespace utilities::strings; - std::string shape = parser.get(Primitive::parse_shape); + Extrinsic extr = Extrinsic::Identity(); + Entity::setRotation(parser, extr); + Entity::setTranslation(parser, extr); - if (iequals(shape, Box::type_name)) - { - return Box::create(parser); - } - if (iequals(shape, Sphere::type_name)) - { - return Sphere::create(parser); - } + std::filesystem::path fpath = parser.get(Parse::file); + float scale = parser.get(Parse::scale, Parse::d_scale); - throw ConstructorError::UnkownType(shape, Primitive::type_name); + return Constructor::create(fpath, extr, scale); } - /// @brief Returns a string help message for a constructing Primitive shapes. - /// @param parser Arguments to pass determine which help information to print. - static std::string help(const utilities::ArgParser& parser) + /// @brief Function to read meshes into a `simulation::Scene`. + /// @param fpath File path to the mesh. + /// @param extr Extrinsic transformation to apply to the mesh. + /// @param scale Scaling factor to apply to the mesh. Default 1. + /// @return A pair with the MeshInfo and the TriangleMesh object. + /// @throws ConstructorError if there was an issue loading the mesh + static std::pair create(std::filesystem::path fpath, + Extrinsic& extr, const float& scale = 1.0f) { - using namespace utilities::strings; - std::string shape = parser.get("-h"); + return Constructor::create(fpath, extr, std::filesystem::path(), scale); + } + + + /// @brief Function to read meshes into a `simulation::Scene`. + /// @param fpath File path to the mesh. + /// @param extr Extrinsic transformation to apply to the mesh. + /// @param extra_search_path One additional path to search for the mesh file at. + /// @param scale Scaling factor to apply to the mesh. Default 1. + /// @return A pair with the MeshInfo and the TriangleMesh object. + /// @throws ConstructorError if there was an issue loading the mesh + static std::pair create(std::filesystem::path fpath, Extrinsic& extr, + std::filesystem::path extra_search_path, + const float& scale = 1.0f) + { + const static std::filesystem::path share_mesh_path = std::filesystem::path(FORGE_SCAN_MESHES_DIR).make_preferred(); + const static bool share_mesh_path_exists = std::filesystem::exists(share_mesh_path); - if (iequals(shape, Box::type_name)) + if (std::filesystem::exists(fpath) == false) { - return Box::helpMessage(); + if (share_mesh_path_exists && + std::filesystem::exists(share_mesh_path / fpath)) + { + fpath = share_mesh_path / fpath; + } + else if (extra_search_path.empty() == false && + std::filesystem::exists(extra_search_path) && + std::filesystem::exists(share_mesh_path / fpath)) + { + fpath = extra_search_path / fpath; + } } - if (iequals(shape, Sphere::type_name)) + + if (std::filesystem::exists(fpath) == false || fpath.has_filename() == false) + { + throw ConstructorError("Cannot load mesh, no file name provided or file does not exist at: " + fpath.string()); + } + + open3d::t::geometry::TriangleMesh mesh; + bool read_success = open3d::t::io::ReadTriangleMesh(fpath.string(), mesh); + + if (read_success == false) { - return Sphere::helpMessage(); + throw ConstructorError("Failed to read mesh file at: " + fpath.string()); } - std::stringstream ss; - ss << Primitive::helpMessage() << "\nPossible shapes are: " - << Box::type_name << ", " - << Sphere::type_name; - return ss.str(); + + mesh.Scale(scale, mesh.GetCenter()); + mesh.Transform(open3d::core::eigen_converter::EigenMatrixToTensor(extr.matrix())); + return {{fpath.make_preferred(), extr}, mesh}; + } + + /// @brief Returns a string help message for constructing a Policy. + /// @param parser Arguments to pass determine which help information to print. + static std::string help([[__maybe_unused__]] const utilities::ArgParser& parser) + { + using namespace utilities::strings; + /// TODO: Return and update this. + return "Help string for Simulation Constructor to be added soon."; } + + /// @brief Describes the flags and options that the Constructor can parse. + struct Parse + { + /// @brief Option. Describes the location of the mesh file. + static constexpr char file[] = "--file"; + + /// @brief Option. Scaling factor for the mesh. + static constexpr char scale[] = "--scale"; + + /// @brief Default. Scaling factor of 1. + static constexpr float d_scale = 1.0f; + + }; }; diff --git a/include/ForgeScan/Simulation/Primitive.hpp b/include/ForgeScan/Simulation/Primitive.hpp deleted file mode 100644 index d81940c..0000000 --- a/include/ForgeScan/Simulation/Primitive.hpp +++ /dev/null @@ -1,213 +0,0 @@ -#ifndef FORGE_SCAN_SIMULATION_PRIMITIVE_HPP -#define FORGE_SCAN_SIMULATION_PRIMITIVE_HPP - -#define H5_USE_EIGEN 1 -#include -#include - -#include "ForgeScan/Common/AABB.hpp" -#include "ForgeScan/Common/Types.hpp" -#include "ForgeScan/Common/Entity.hpp" -#include "ForgeScan/Common/Exceptions.hpp" - -// Helper constant for saving data in HDF5 files. -// Used by derived classes and Scene. -// Undefined at the end of Scene.hpp. -#define FS_HDF5_PRIMITIVE_TYPE_NAME_ATTR "type_name" - - -namespace forge_scan { -namespace simulation { - - -/// @brief Base class for Primitive geometry types. -struct Primitive : Entity -{ - /// @details Required to call print method. - friend std::ostream& operator<<(std::ostream&, const Primitive&); - -public: - virtual ~Primitive() { } - - - /// @return Help message for constructing a Primitive with ArgParser. - static std::string helpMessage() - { - return "A primitive shape may be added with the following arguments:" - "\n\t" + Primitive::help_string + - "\n\nFor details on primitive shape options, enter \"-h \"."; - } - - - // ***************************************************************************************** // - // * PUBLIC VIRTUAL METHODS * // - // ***************************************************************************************** // - - - /// @brief Returns the type name of the derived Primitive class. - virtual const std::string& getTypeName() const = 0; - - - /// @brief Determines if, and where, the line between the start and end points first intersects - /// the geometry. - /// @param start Start point, relative to the derived class's reference frame. - /// @param end End point, relative to the derived class's reference frame. - /// @param [out] t Parameterization value for the intersection. Values `0<=t<=1` are valid - /// on the line segment where `t=0` is the start point and `t=1` is the end point. - /// @return True if the line intersects and does so in a valid region of the line. - /// @note If the line DOES NOT intersect we return false with t unchanged. - virtual bool hit(const Point& start, const Point& end, float& t) const = 0; - - - /// @brief Calculates is the point is inside the Primitive. - /// @param input Point in space. - /// @param extr Transformation from the world frame to the frame the `input` is in. - /// @return True if the point is inside, false if not. - virtual bool isInside(const Point& input, const Extrinsic& extr) const = 0; - - - /// @brief Calculates is the point is inside the Primitive. - /// @param input Point in space. - /// @param extr Transformation from the world frame to the frame the `input` is in. - /// @param [out] input_this_f The point transformed to the Primitive's frame. Useful if - /// calling `getSignedDistance` or `getNearestSurfacePoint` after this. - /// @return True if the point is inside, false if not. - virtual bool isInside(const Point& input, const Extrinsic& extr, Point& input_this_f) const = 0; - - - /// @brief Calculates the shortest, signed distance from the point and the Primitive's surface. - /// @param input Point in space. - /// @param extr Transformation from the world frame to the frame the `input` is in. - /// @return The shortest distance between the point and the surface with negative distances - /// being inside the Primitive geometry. - virtual float getSignedDistance(const Point& input, const Extrinsic& extr) const = 0; - - - /// @brief Calculates the shortest, signed distance from the point and the Primitive's surface. - /// @param input Point in space, relative to the derived class's reference frame. - /// @return The shortest distance between the point and the surface with negative distances - /// being inside the Primitive geometry. - virtual float getSignedDistance(const Point& input) const = 0; - - - /// @brief Calculates the location on the Primitive surface closest to the input. - /// @param input Point in space. - /// @param extr Transformation from the world frame to the frame the `input` is in. - /// @return A Point located on the primitive's surface. - virtual Point getNearestSurfacePoint(const Point& input, const Extrinsic& extr) const = 0; - - - /// @brief Calculates the location on the Primitive surface closest to the input. - /// @param input Point in space, relative to the derived class's reference frame. - /// @return A Point located on the primitive's surface. - virtual Point getNearestSurfacePoint(const Point& input) const = 0; - - - /// @brief Saves the shapes contents into an HDF5 file format. - /// @param g_primitive Group to add data to. - virtual void save(HighFive::Group& g_primitive) const = 0; - - - - // ***************************************************************************************** // - // * PUBLIC CLASS MEMBERS * // - // ***************************************************************************************** // - - - /// @brief Bounding box limits for the geometric primitive. - /// Positions are relative to the derived class's reference frame. - const Point upperAABBbound, lowerAABBbound; - - static const std::string parse_name, parse_shape; - - static const std::string help_string; - - static const std::string type_name; - - -protected: - // ***************************************************************************************** // - // * PROTECTED CLASS METHODS * // - // ***************************************************************************************** // - - - /// @brief Protected constructor for derived classes only. - /// @param extr Transformation from the world frame to the derived class's reference frame. - /// @param upperAABBbound Upper bound position, relative to the derived class's reference frame. - /// @param lowerAABBbound Lower bound position, relative to the derived class's reference frame. - Primitive(const Extrinsic& extr, const Point& upperAABBbound, const Point& lowerAABBbound) - : Entity(extr), - upperAABBbound(upperAABBbound), - lowerAABBbound(lowerAABBbound) - { - - } - - - /// @brief Checks if a point is within the axis-aligned bounding box (AABB) for the Primitive. - /// @param input Point to check. - /// @return True if the point is between the upper and lower AABB bound (or on one of them). False else. - bool insideBounds(const Point& input) const - { - return (input.array() >= lowerAABBbound.array()).all() && (input.array() <= upperAABBbound.array()).all(); - } - - - - /// @brief Quick axis-aligned bounding box (AABB) check for bounding box intersection of a - //// geometric primitive. This prevents needless intersection checks for some rays. - /// @param start Starting point of the ray, relative to the derived class's reference frame. - /// @param end Ending point of the ray, relative to the derived class's reference frame. - /// @param [out] t Scaling factor for the ray to intersect the AABB. Describes when the ray first - /// hits a face of the AABB. Values 0 <= t <= 1 are valid on the line segment - /// between start and end. - /// @return True if the ray has any intersection with the primitive's bounding box. - /// @warning The output variable, `t`, is valid only when this function returns true. - /// Otherwise it does not describe an intersection and should not be trusted. - bool hitAABB(const Point& start, const Point& end, float& t) const - { - float tmax; - const Ray inverse_ray = (end - start).cwiseInverse(); - return AABB::find_bounded_intersection(lowerAABBbound, upperAABBbound, start, inverse_ray, 0, 1, t, tmax); - } - - - /// @brief Prints information about the Primitive to the output stream. - /// @param out Output stream to write to. - virtual void print(std::ostream& out) const - { - out << "Primitive at (" << this->extr.translation().transpose() << ")"; - } -}; - - -/// @brief Prints info about the Primitive to the output stream. -/// @param out Output stream to write to. -/// @param primitive Primitive to write to the output stream -/// @return Reference to the output stream. -std::ostream& operator<<(std::ostream &out, const Primitive& primitive) -{ - primitive.print(out); - return out; -} - - -/// @brief String for the class name. -const std::string Primitive::type_name = "Primitive"; - -/// @brief ArgParser key for the dictionary name of a shape. -const std::string Primitive::parse_name = "--name"; - -/// @brief ArgParser key for the type name of of a shape. -const std::string Primitive::parse_shape = "--shape"; - -/// @brief String explaining what arguments this class accepts. -const std::string Primitive::help_string = - "--name --shape [shape-specific options]"; - - -} // namespace simulation -} // namespace forge_scan - - -#endif // FORGE_SCAN_SIMULATION_PRIMITIVE_HPP diff --git a/include/ForgeScan/Simulation/Scene.hpp b/include/ForgeScan/Simulation/Scene.hpp index 32e22fc..3ec8315 100644 --- a/include/ForgeScan/Simulation/Scene.hpp +++ b/include/ForgeScan/Simulation/Scene.hpp @@ -9,6 +9,9 @@ #define H5_USE_EIGEN 1 #include +#include + +#include #include "ForgeScan/Common/Definitions.hpp" #include "ForgeScan/Common/Entity.hpp" @@ -24,12 +27,16 @@ // Define some helper constants for HDF5. // These are undefined at the end of this header. #define FS_HDF5_SCENE_GROUP "Scene" + #define FS_HDF5_SCAN_LOWER_BOUND_DSET "/" FS_HDF5_SCENE_GROUP "/" "scan_lower_bound" #define FS_HDF5_GRID_SIZE_ATTR "size" #define FS_HDF5_GRID_RESOLUTION_ATTR "resolution" #define FS_HDF5_GRID_DIMENSIONS_ATTR "dimension" -#define FS_HDF5_SHAPES_GROUP "Shapes" -#define FS_HDF5_SHAPES_SUB_GROUP "/" FS_HDF5_SCENE_GROUP "/" FS_HDF5_SHAPES_GROUP + +#define FS_HDF5_MESHES_GROUP "Meshes" +#define FS_HDF5_MESHES_EXTR_SUFFIX "extrinsic" +#define FS_HDF5_MESHES_FILEPATH "filepath" + #define FS_HDF5_GROUND_TRUTH_GROUP "GroundTruth" #define FS_HDF5_OCCUPANCY_DSET "Occupancy" #define FS_HDF5_TSDF_DSET "TSDF" @@ -39,10 +46,10 @@ namespace forge_scan { namespace simulation { -/// @brief A collection of Primitive objects which are imaged together in the same scene. +/// @brief A collection of triangle mesh objects which are imaged together in the same scene. struct Scene { - /// @details Required to print the shape dictionary. + /// @details Required to print the Scene's contents. friend std::ostream& operator<<(std::ostream &, const Scene&); public: @@ -79,14 +86,14 @@ struct Scene auto g_scene = file.createGroup(FS_HDF5_SCENE_GROUP); H5Easy::dump(file, FS_HDF5_SCAN_LOWER_BOUND_DSET, this->scan_lower_bound.matrix()); - auto g_shape = g_scene.createGroup(FS_HDF5_SHAPES_GROUP); + auto g_meshes = g_scene.createGroup(FS_HDF5_MESHES_GROUP); - for (const auto& dict_item : this->shapes_map) + int n = 0; + for (const auto& item : this->mesh_list) { - auto g_primitive = g_shape.createGroup(dict_item.first); - dict_item.second->save(g_primitive); - const std::string dset_path = FS_HDF5_SHAPES_SUB_GROUP "/" + dict_item.first + "/extr"; - H5Easy::dump(file, dset_path, dict_item.second->getExtr().matrix()); + auto g_mesh = g_meshes.createGroup(std::to_string(n++)); + g_mesh.createAttribute(FS_HDF5_MESHES_FILEPATH, item.first.fpath.string()); + Scene::writeExtrToHDF5(file, g_mesh.getPath(), item.first.extr); } if (this->grid_properties) @@ -131,40 +138,25 @@ struct Scene auto scene_groups = g_scene.listObjectNames(); - if(std::find(scene_groups.begin(), scene_groups.end(), FS_HDF5_SHAPES_GROUP) != scene_groups.end()) + if(std::find(scene_groups.begin(), scene_groups.end(), FS_HDF5_MESHES_GROUP) != scene_groups.end()) { // Only clear all elements if we make it this far into loading the HDF5. - this->shapes_map.clear(); + this->mesh_list.clear(); - auto g_shapes = g_scene.getGroup(FS_HDF5_SHAPES_GROUP); - auto primitive_groups = g_shapes.listObjectNames(); + auto g_meshes = g_scene.getGroup(FS_HDF5_MESHES_GROUP); + auto mesh_groups = g_meshes.listObjectNames(); // Begin reading each shape and getting the attributes. - for (const auto& shape_name : primitive_groups) + for (const auto& group_name : mesh_groups) { - auto g_primitive = g_shapes.getGroup(shape_name); - const std::string type_name = g_primitive.getAttribute(FS_HDF5_PRIMITIVE_TYPE_NAME_ATTR).read(); - - // Use a string stream to simulate parsed arguments from a user. - // Totally not 'efficient' but what is efficiency anyway? - std::stringstream ss("--name"); - ss << "--name " << shape_name << " --shape " << type_name; - if (type_name == "Sphere") - { - ss << " --radius " << g_primitive.getAttribute(FS_HDF5_SPHERE_R_ATTR).read(); - } - else if (type_name == "Box") - { - ss << " --l " << g_primitive.getAttribute(FS_HDF5_BOX_L_ATTR).read(); - ss << " --w " << g_primitive.getAttribute(FS_HDF5_BOX_W_ATTR).read(); - ss << " --h " << g_primitive.getAttribute(FS_HDF5_BOX_H_ATTR).read(); - } - this->add(ss.str()); + auto g_mesh = g_meshes.getGroup(group_name); + std::filesystem::path mesh_fpath = std::filesystem::path(g_mesh.getAttribute(FS_HDF5_MESHES_FILEPATH).read()); Extrinsic extr; - const std::string dset_path = FS_HDF5_SHAPES_SUB_GROUP "/" + shape_name + "/extr"; - extr.matrix() = H5Easy::load(file, dset_path); - this->transform(shape_name, extr, true); + Scene::readExtrFromHDF5(file, g_mesh.getPath(), extr); + + this->mesh_list.emplace_back( Constructor::create(mesh_fpath, extr, fpath) ); + this->o3d_scene.AddTriangles(this->mesh_list.back().second); } } @@ -194,67 +186,17 @@ struct Scene } - - // ***************************************************************************************** // - // * PUBLIC PRIMITIVE METHODS * // - // ***************************************************************************************** // - - - /// @brief Adds a new Primitive Shape to the scene. - /// @param parser ArgParser with parameters for the Primitive Shape. + /// @brief Adds a new mesh to the scene. + /// @param parser ArgParser with parameters for the mesh to add. /// @throws InvalidMapKey If no name was provided. /// @throws InvalidMapKey If a shape with the same name already exists. void add(const utilities::ArgParser& parser) { - const std::string name = parser.get(Primitive::parse_name); - if (name.empty()) - { - throw InvalidMapKey::NoNameProvided(); - } - else if (this->shapes_map.count(name) != 0) - { - throw InvalidMapKey::NameAlreadyExists(name); - } - this->shapes_map.insert( {name, Constructor::create(parser)} ); + this->mesh_list.emplace_back(Constructor::create(parser)); + this->o3d_scene.AddTriangles(this->mesh_list.back().second); } - /// @brief Removes a Primitive Shape from the Scene. - /// @param name Name of the Shape to remove. - /// @return True if an item was removed. False if no Shape with that name exists. - bool remove(const std::string& name) - { - if (this->shapes_map.erase(name) != 0) - { - return true; - } - return false; - } - - - /// @brief Transforms a Primitive Shape's pose in the Scene. - /// @param name Name of the Shape to transform. - /// @param extr Transformation to apply to the Shape. - /// @param world True for world frame transformation. False for body frame. Default false. - /// @return True if an item was removed. False if no shape with that name exists. - bool transform(const std::string& name, const Extrinsic& extr, const bool& world = false) - { - if (auto search = this->shapes_map.find(name); search != this->shapes_map.end()) { - if (world) - { - search->second->transformWorldFrame(extr); - } - else - { - search->second->transformBodyFrame(extr); - } - return true; - } - return false; - } - - - // ***************************************************************************************** // // * PUBLIC CAMERA METHODS * // // ***************************************************************************************** // @@ -271,38 +213,29 @@ struct Scene void image(const std::shared_ptr& camera, const bool& pose_is_world_frame = false) { - // The camera's origin position in its own reference frame is always (0,0,0). - static const Point origin_camera_f = Point::Zero(); - // If we are placing the camera relative to the world frame then the pose is what was provided. // But if the pose is relative to the scene frame, then transform it to be in the world frame. - Extrinsic camera_pose = pose_is_world_frame ? camera->getExtr() : this->scan_lower_bound * camera->getExtr(); + Extrinsic camera_pose = pose_is_world_frame ? camera->extr : this->scan_lower_bound * camera->extr; - camera->resetDepth(); - for (size_t row = 0; row < camera->intr->height; ++row) + open3d::core::Tensor rays({static_cast(camera->intr->height), static_cast(camera->intr->width), 6}, open3d::core::Float32); + Eigen::Map rays_map(rays.GetDataPtr(), 6, camera->intr->size()); + + Eigen::Matrix r; + r.topRows<3>() = camera_pose.translation(); + int64_t linear_idx = 0; + for (size_t y = 0; y < camera->intr->height; ++y) { - for (size_t col = 0; col < camera->intr->width; ++col) + for (size_t x = 0; x < camera->intr->width; ++x, ++linear_idx) { - const Point sensed_camera_f = camera->getPoint(row, col); - float min_scale = 1; - for (const auto& item : this->shapes_map) - { - float scale = 1; - Point origin_shapes_f = item.second->toThisFromOther(origin_camera_f, camera_pose); - Point sensed_shapes_f = item.second->toThisFromOther(sensed_camera_f, camera_pose); - if (item.second->hit(origin_shapes_f, sensed_shapes_f, scale)) - { - min_scale = std::max(std::min(scale, min_scale), 0.0f); - } - } - if (min_scale < 1) - { - camera->image(row, col) *= min_scale; - } + Eigen::Vector3f ray_dir = camera_pose.rotation() * camera->getPoint(y, x).normalized(); + r.bottomRows<3>() = ray_dir; + rays_map.col(linear_idx) = r; } } + + auto result = this->o3d_scene.CastRays(rays); + camera->image = open3d::core::eigen_converter::TensorToEigenMatrixXf(result["t_hit"]); camera->addNoise(); - camera->saturateDepth(); } @@ -326,37 +259,21 @@ struct Scene { if (this->grid_properties.get() == nullptr) { - // Use default Grid Properties if non were set. this->grid_properties = Grid::Properties::createConst(); } this->true_occupancy = metrics::ground_truth::Occupancy::create(this->grid_properties); - const float res = this->true_occupancy->properties->resolution; - const float half_res = res * 0.5; - const size_t nz = this->true_occupancy->properties->size.z(); - const size_t ny = this->true_occupancy->properties->size.y(); - const size_t nx = this->true_occupancy->properties->size.x(); + const size_t n_voxels = this->grid_properties->getNumVoxels(); + open3d::core::Tensor voxel_centers = this->getVoxelCenters(); - // Current voxel position, relative to scan_lower_bound frame. - Point voxel_scan_f = Point::Zero(); - size_t n = 0; - for (size_t z = 0; z < nz; ++z) + auto result = this->o3d_scene.ComputeSignedDistance(voxel_centers, 0, 5); + result = result.Reshape({1, static_cast(n_voxels)}); + + Eigen::MatrixXi result_eigen = open3d::core::eigen_converter::TensorToEigenMatrixXi(result); + for (size_t i = 0; i < n_voxels; ++i) { - for (size_t y = 0; y < ny; ++y) - { - for (size_t x = 0; x < nx; ++x) - { - this->true_occupancy->operator[](n) = this->voxelOccupied(voxel_scan_f, half_res); - ++n; - voxel_scan_f.x() += res; - } - // Re-set our X-position. - voxel_scan_f.x() = 0; - voxel_scan_f.y() += res; - } - // Re-set our Y-position. - voxel_scan_f.y() = 0; - voxel_scan_f.z() += res; + this->true_occupancy->operator[](i) = result_eigen(0, i) == 1 ? VoxelOccupancy::OCCUPIED : + VoxelOccupancy::FREE; } } @@ -367,36 +284,20 @@ struct Scene { if (this->grid_properties.get() == nullptr) { - // Use default Grid Properties if non were set. this->grid_properties = Grid::Properties::createConst(); } this->true_tsdf = metrics::ground_truth::TSDF::create(this->grid_properties); - const float res = this->true_tsdf->properties->resolution; - const size_t nz = this->true_tsdf->properties->size.z(); - const size_t ny = this->true_tsdf->properties->size.y(); - const size_t nx = this->true_tsdf->properties->size.x(); + const size_t n_voxels = this->grid_properties->getNumVoxels(); + open3d::core::Tensor voxel_centers = this->getVoxelCenters(); - // Current voxel position, relative to scan_lower_bound frame. - Point voxel_scan_f = Point::Zero(); - size_t n = 0; - for (size_t z = 0; z < nz; ++z) + auto result = this->o3d_scene.ComputeSignedDistance(voxel_centers, 0, 5); + result = result.Reshape({1, static_cast(n_voxels)}); + + Eigen::MatrixXd result_eigen = open3d::core::eigen_converter::TensorToEigenMatrixXd(result); + for (size_t i = 0; i < n_voxels; ++i) { - for (size_t y = 0; y < ny; ++y) - { - for (size_t x = 0; x < nx; ++x) - { - this->true_tsdf->operator[](n) = this->voxelSignedDistance(voxel_scan_f); - ++n; - voxel_scan_f.x() += res; - } - // Re-set our X-position. - voxel_scan_f.x() = 0; - voxel_scan_f.y() += res; - } - // Re-set our Y-position. - voxel_scan_f.y() = 0; - voxel_scan_f.z() += res; + this->true_tsdf->operator[](i) = result_eigen(0, i); } } @@ -524,79 +425,74 @@ struct Scene } + /// @brief Writes an Extrinsic eigen matrix to an HDF5 file with HighFive. + /// @param file HDF5 file to use. + /// @param mesh_group_path Path to the matrix location. + /// @param extr The extrinsic matrix to save. + static void writeExtrToHDF5(HighFive::File& file, const std::string& mesh_group_path, const Extrinsic& extr) + { + H5Easy::dump(file, mesh_group_path + "/" FS_HDF5_MESHES_EXTR_SUFFIX, extr.matrix()); + } + + + /// @brief Reads an Extrinsic eigen matrix from an HDF5 file with HighFive. + /// @param file HDF5 file to use. + /// @param mesh_group_path Path to the matrix location. + /// @param extr The extrinsic matrix to read. + static void readExtrFromHDF5(HighFive::File& file, const std::string& mesh_group_path, Extrinsic& extr) + { + extr.matrix() = H5Easy::load(file, mesh_group_path + "/" FS_HDF5_MESHES_EXTR_SUFFIX); + } + + /*********************************************************************************************/ /* PRIVATE GROUND TRUTH METHODS */ /*********************************************************************************************/ - /// @brief Checks if the voxel is occupied by any Shapes in the Scene. - /// @param center Voxel center location, relative to the scan_lowe_bound frame. - /// @param half_res Half of the voxel resolution; the distance from the center to any face. - /// @return True if the voxel is occupied. - VoxelOccupancy voxelOccupied(const Point& center, const float& half_res) const + /// @brief Gets a list of voxel center location to test for occupancy or distance. + /// @return A tensor of shape {n, 3} where n is the number of voxels in the grid. The tensor is + /// ordered first in x, then y, then z. + open3d::core::Tensor getVoxelCenters() { - Point center_primitive_f; - VoxelOccupancy result = VoxelOccupancy::FREE; - for (const auto& item : this->shapes_map) + const size_t n_voxels = this->grid_properties->getNumVoxels(); + + open3d::core::Tensor voxel_centers({static_cast(n_voxels), 3}, open3d::core::Float32); + Eigen::Map voxel_centers_map(voxel_centers.GetDataPtr(), 3, n_voxels); + + size_t linear_idx = 0; + Point voxel_scan_f = Point::Zero(); + for (size_t z = 0; z < this->grid_properties->size.z(); ++z) { - if (item.second->isInside(center, this->scan_lower_bound, center_primitive_f)) - { - // Mark as fully inside at least one shape. Can exit after the first is found. - result = VoxelOccupancy::OCCUPIED; - break; - } - else + for (size_t y = 0; y < this->grid_properties->size.y(); ++y) { - Translation to_surface = (item.second->getNearestSurfacePoint(center_primitive_f) - center_primitive_f).cwiseAbs(); - if ((to_surface.array() < half_res).all()) + for (size_t x = 0; x < this->grid_properties->size.x(); ++x, ++linear_idx) { - // Mark as clipped by at least one shape. - // But must keep searching to see if we are inside any. - result = VoxelOccupancy::CLIPPED; + voxel_centers_map.col(linear_idx) = this->scan_lower_bound * voxel_scan_f.homogeneous(); + voxel_scan_f.x() += this->grid_properties->resolution; } + voxel_scan_f.x() = 0; + voxel_scan_f.y() += this->grid_properties->resolution; } + voxel_scan_f.y() = 0; + voxel_scan_f.z() += this->grid_properties->resolution; } - return result; - } - - /// @return The signed distance for the voxel center point. - - /// @brief Calculates the signed distance from the voxel to the closest Shape surface in - /// the Scene. - /// @param center Voxel center location, relative to the scan_lowe_bound frame. - /// @return Sign distance for the voxel. - double voxelSignedDistance(const Point& center) - { - float dist = -1 * std::numeric_limits::infinity(); - Point center_primitive_f; - for (const auto& item : this->shapes_map) - { - /// TODO: I do not know how to handle the distance when inside multiple arbitrary shapes - if (item.second->isInside(center, this->scan_lower_bound, center_primitive_f)) - { - // Fully inside at least one shape is, for now, infinitely inside all shapes. - return -1 * std::numeric_limits::infinity(); - } - // If we are outside this item then record its distance. - float dist_item = item.second->getSignedDistance(center, this->scan_lower_bound); - - // then store the smallest distance we hae seen so far. - dist = utilities::math::smallest_magnitude(dist, dist_item); - } - return dist; + return voxel_centers; } - // ***************************************************************************************** // // * PRIVATE CLASS MEMBERS * // // ***************************************************************************************** // - /// @brief Dictionary of names to Primitive Shapes. - std::map> shapes_map; + /// @brief Open3D's raycasting implementation. + open3d::t::geometry::RaycastingScene o3d_scene; + + /// @brief List of information about the meshes in the scene and the mesh itself. + std::list> mesh_list; /// @brief Shared reference to a Ground Truth Occupancy Grid for the Scene. std::shared_ptr true_occupancy{nullptr}; @@ -612,12 +508,16 @@ struct Scene /// @return Reference to the output stream. std::ostream& operator<<(std::ostream &out, const Scene& scene) { - if (!scene.shapes_map.empty()) + if (!scene.mesh_list.empty()) { out << "Scene contains:"; - for (const auto& dict_item : scene.shapes_map) + for (const auto& item : scene.mesh_list) { - out << "\n\t" << dict_item.first << ": " << *dict_item.second; + std::string description = item.second.ToString(); + std::replace(description.begin(), description.end(), '\n', ' '); + out << "\n\tMesh name: " << item.first.fpath.stem() + << "\n\tFrom file: " << item.first.fpath + << "\n\tWith properties:" << description; } } else @@ -637,16 +537,12 @@ std::ostream& operator<<(std::ostream &out, const Scene& scene) #undef FS_HDF5_GRID_SIZE_ATTR #undef FS_HDF5_GRID_RESOLUTION_ATTR #undef FS_HDF5_GRID_DIMENSIONS_ATTR -#undef FS_HDF5_SHAPES_GROUP +#undef FS_HDF5_MESHES_GROUP +#undef FS_HDF5_MESHES_EXTR_SUFFIX +#undef FS_HDF5_MESHES_FILEPATH #undef FS_HDF5_GROUND_TRUTH_GROUP #undef FS_HDF5_OCCUPANCY_DSET #undef FS_HDF5_TSDF_DSET -// Undefine the definitions from Primitive.hpp, Box.hpp, and Sphere.hpp: -#undef FS_HDF5_SHAPE_TYPE_NAME_ATTR -#undef FS_HDF5_SPHERE_R_ATTR -#undef FS_HDF5_BOX_L_ATTR -#undef FS_HDF5_BOX_W_ATTR -#undef FS_HDF5_BOX_H_ATTR #endif // FORGE_SCAN_SIMULATION_SCENE_HPP diff --git a/include/ForgeScan/Simulation/Sphere.hpp b/include/ForgeScan/Simulation/Sphere.hpp deleted file mode 100644 index a4fbe62..0000000 --- a/include/ForgeScan/Simulation/Sphere.hpp +++ /dev/null @@ -1,276 +0,0 @@ -#ifndef FORGE_SCAN_SIMULATION_SPHERE_HPP -#define FORGE_SCAN_SIMULATION_SPHERE_HPP - -#include -#include - -#include "ForgeScan/Simulation/Primitive.hpp" -#include "ForgeScan/Utilities/ArgParser.hpp" - -// Helper constant for saving data in HDF5 files. -// Used in this class and by Scene. -// Undefined at the end of Scene.hpp. -#define FS_HDF5_SPHERE_R_ATTR "radius" - - -namespace forge_scan { -namespace simulation { - - -/// @brief A simple analytical sphere. -/// @note The reference frame for the Sphere is located at its center. -struct Sphere : public Primitive -{ - /// @brief Constructs an analytical sphere with the given radius at the specified location. - /// @param radius Radius value for the sphere. Default is 1 unit. - /// @param extr Transformation to the world frame to the center of the Sphere. - Sphere(const float& radius = 1, const Extrinsic& extr = Extrinsic::Identity()) - : Primitive(extr, - getAABBbound(std::abs(radius)), - getAABBbound(-1 * std::abs(radius))), - radius(std::abs(radius)), - radius_squared(this->radius * this->radius) - { - - } - - - /// @brief Constructor for a shared pointer to a Sphere. - /// @param radius Radius value for the sphere. Default is 1 unit. - /// @param extr Transformation to the world frame to the center of the Sphere. - /// @return Shared pointer to a Sphere. - static std::shared_ptr create(const float& radius = 1, - const Extrinsic& extr = Extrinsic::Identity()) - { - return std::shared_ptr(new Sphere(radius, extr)); - } - - - /// @brief Constructor for a shared pointer to a Sphere. - /// @param parser ArgParser with arguments to construct a Sphere from. - /// @return Shared pointer to a Sphere. - static std::shared_ptr create(const utilities::ArgParser& parser) - { - Extrinsic extr = Extrinsic::Identity(); - Entity::setTranslation(parser, extr); - return std::shared_ptr(new Sphere(parser.get(Sphere::parse_radius, Sphere::default_radius), extr)); - } - - - /// @return Help message for constructing a Sphere with ArgParser. - static std::string helpMessage() - { - return "A Sphere may be added with the following arguments:" - "\n\t" + Sphere::help_string + - "\nIf the optional arguments are not provided, the default values are:" - "\n\t" + Sphere::default_arguments; - } - - - // ***************************************************************************************** // - // * PUBLIC VIRTUAL METHOD OVERRIDES * // - // ***************************************************************************************** // - - - const std::string& getTypeName() const override final - { - return Sphere::type_name; - } - - - bool hit(const Point& start, const Point& end, float& t) const override final - { - float unused_intersection_time; - if ( !hitAABB(start, end, unused_intersection_time) ) - { - return false; - } - - // Adapted from https://stackoverflow.com/questions/6533856 with a partial quadratic solver - // for only the real-valued solutions of the intersection. - // Note that we require the start/end point to be relative to the Sphere's reference frame - // thus the value of "center" used in the references above is always equal to zero here. - // See also: http://paulbourke.net/geometry/circlesphere/. - - // Quadratic equation for intersection: - // 0 = A*(x*x) + B*x + C - float A = (start - end).array().pow(2).sum(); - float C = start.array().pow(2).sum() - radius_squared; - float B = end.array().pow(2).sum() - A - C - radius_squared; - - // Find quadratic equation determinant. - // Early exit if negative; a complex solutions mean no intersection. - float D = B*B - 4*A*C; - if (D < 0) - { - return false; - } - - // Pre-calculations help us optimize the quadratic formula. And checking the sign of B lets - // us utilize an numerically stable form in which only addition OR subtraction is required. - // D = sqrt(B*B - 4*A*C) - // if B < 0 - // X_1 = (-B + D) / 2*A - // X_2 = 2*C / (-B + D) - // (Leads to adding two positives) - // if B >= 0 - // X_1 = (-B - D) / 2*A - // X_2 = 2*C / (-B - D) - // In short, the first case lets us add two positives and the second lets us subtract two - // negatives. This is ideal as it avoids any case where we subtract quantities with the - // same sign. In cases where these values are similar in magnitude (for this case, when - // 4*A*C is small) this leads to imprecision in rounding. For details on this numeric - // stability see: https://people.csail.mit.edu/bkph/articles/Quadratics.pdf - - // Both cases require the following values which we may pre-compute - D = std::sqrt(D); - A *= 2; - C *= 2; - B *= -1; - - if (B > 0) - { - B += D; - } - else - { - B -= D; - } - t = C / B; - float x = B / A; - - if (t >= 0) - { - // Find minimum if both are positive. Else, leave t unchanged as the other solution is non-positive. - if (x > 0) t = std::min(t, x); - } - else - { - // Find maximum if both are negative. Else, set t to the other solution, which must be non-negative. - t = x < 0 ? std::max(t, x) : x; - } - // For the case t == 0 the answers are the same so no comparison is needed. - - // In this, 0 <= t <= 1 indicates a point between the two values of interest. - return ( 0 <= t && t <= 1 ); - } - - - virtual bool isInside(const Point& input, const Extrinsic& extr) const override final - { - return this->getSignedDistance(input, extr) < 0; - } - - - virtual bool isInside(const Point& input, const Extrinsic& extr, Point& input_this_f) const override final - { - input_this_f = this->getToThisFromOther(extr) * input.homogeneous(); - return this->getSignedDistance(input_this_f) < 0; - } - - - float getSignedDistance(const Point& input, const Extrinsic& extr) const override final - { - return this->getSignedDistance(this->getToThisFromOther(extr) * input.homogeneous()); - } - - - float getSignedDistance(const Point& input) const override final - { - return input.norm() - radius; - } - - - Point getNearestSurfacePoint(const Point& input, const Extrinsic& extr) const override final - { - return this->getNearestSurfacePoint(this->getToThisFromOther(extr) * input.homogeneous()); - } - - - Point getNearestSurfacePoint(const Point& input) const override final - { - return input.array() * (radius / input.norm()); - } - - - - // ***************************************************************************************** // - // * PUBLIC CLASS MEMBERS * // - // ***************************************************************************************** // - - - /// @brief Sphere radius in world units. - const float radius; - - static const float default_radius; - - static const std::string parse_radius; - - static const std::string help_string, default_arguments; - - static const std::string type_name; - -private: - // ***************************************************************************************** // - // * PRIVATE CLASS METHODS * // - // ***************************************************************************************** // - - - /// @brief Constructor helper for generating a sphere's AABB bounds. - /// @param radius Radius of the sphere. - /// Pass as positive for the upper bound. Pass as negative for the lower bound. - /// @return Axis-aligned bounding box point for the upper or lower, depending on the radius' sign. - static Point getAABBbound(const float& radius) - { - return Point(radius, radius, radius); - } - - /// @brief Saves the shapes contents into an HDF5 file format. - /// @param g_primitive Group to add data to. - void save(HighFive::Group& g_primitive) const override final - { - g_primitive.createAttribute(FS_HDF5_PRIMITIVE_TYPE_NAME_ATTR, this->getTypeName()); - g_primitive.createAttribute(FS_HDF5_SPHERE_R_ATTR, this->radius); - } - - - /// @brief Prints information about the Sphere to the output stream. - /// @param out Output stream to write to. - void print(std::ostream& out) const override final - { - out << this->getTypeName() << " "; - Primitive::print(out); - out << " with radius " << this->radius; - } - - - /// @brief Helper constant for the ray hit calculation. - const float radius_squared; -}; - - -/// @brief String for the class name. -const std::string Sphere::type_name = "Sphere"; - -/// @brief Default radius value. -const float Sphere::default_radius = 1.0; - -/// @brief ArgParser key for the Sphere radius. -const std::string Sphere::parse_radius = "--radius"; - -/// @brief String explaining what arguments this class accepts. -const std::string Sphere::help_string = - Sphere::translation_help_string + " [" + Sphere::parse_radius + " ]"; - -/// @brief String explaining what this class's default parsed values are. -const std::string Sphere::default_arguments = - Sphere::translation_default_arguments + " " + - Sphere::parse_radius + " " + std::to_string(Sphere::default_radius); - - - -} // namespace simulation -} // namespace forge_scan - - -#endif // FORGE_SCAN_SIMULATION_SPHERE_HPP diff --git a/src/Examples/Scene/main.cpp b/src/Examples/Scene/main.cpp index 6358db0..683fb4e 100644 --- a/src/Examples/Scene/main.cpp +++ b/src/Examples/Scene/main.cpp @@ -15,9 +15,9 @@ int main(const int argc, const char **argv) auto scene = forge_scan::simulation::Scene::create(scene_lower_bound); - scene->add("--name sphere1 --shape sphere --radius 0.35"); - scene->add("--name sphere2 --shape sphere --radius 0.25 --x 0.25 --y 0.25 --z 0.25"); - scene->add("--name box1 --shape box --l 1.25 --w 0.25 --h 0.75 --rx 6"); + scene->add("--file abc0.stl --x 0.5"); + scene->add("--file abc1.stl --z 0.5 --scale 5"); + scene->add("--file abc2.stl --y 0.5 --scale 5"); // ****************************** Calculate the ground truth ******************************* //